From: Phillip Potter <phil@philpotter.co.uk>
To: gregkh@linuxfoundation.org
Cc: Larry.Finger@lwfinger.net, dan.carpenter@oracle.com,
linux-kernel@vger.kernel.org, linux-staging@lists.linux.dev,
fabioaiuto83@gmail.com, martin@kaiser.cx
Subject: [PATCH v4 1/6] staging: r8188eu: introduce new core dir for RTL8188eu driver
Date: Wed, 28 Jul 2021 00:22:14 +0100 [thread overview]
Message-ID: <20210727232219.2948-2-phil@philpotter.co.uk> (raw)
In-Reply-To: <20210727232219.2948-1-phil@philpotter.co.uk>
This patchset is split in order to keep the file sizes down. This core
directory is part of the newer/better driver from GitHub modified by
Larry Finger. Import this as the basis for all future work going
forward.
Suggested-by: Larry Finger <Larry.Finger@lwfinger.net>
Signed-off-by: Phillip Potter <phil@philpotter.co.uk>
---
drivers/staging/r8188eu/core/rtw_ap.c | 1976 ++++
drivers/staging/r8188eu/core/rtw_br_ext.c | 1184 +++
drivers/staging/r8188eu/core/rtw_cmd.c | 2206 +++++
drivers/staging/r8188eu/core/rtw_debug.c | 943 ++
drivers/staging/r8188eu/core/rtw_efuse.c | 872 ++
drivers/staging/r8188eu/core/rtw_ieee80211.c | 1625 ++++
drivers/staging/r8188eu/core/rtw_io.c | 323 +
drivers/staging/r8188eu/core/rtw_ioctl_set.c | 1118 +++
drivers/staging/r8188eu/core/rtw_iol.c | 209 +
drivers/staging/r8188eu/core/rtw_led.c | 1705 ++++
drivers/staging/r8188eu/core/rtw_mlme.c | 2354 +++++
drivers/staging/r8188eu/core/rtw_mlme_ext.c | 8407 ++++++++++++++++++
drivers/staging/r8188eu/core/rtw_mp.c | 1000 +++
drivers/staging/r8188eu/core/rtw_mp_ioctl.c | 1352 +++
drivers/staging/r8188eu/core/rtw_p2p.c | 2068 +++++
drivers/staging/r8188eu/core/rtw_pwrctrl.c | 655 ++
drivers/staging/r8188eu/core/rtw_recv.c | 2252 +++++
drivers/staging/r8188eu/core/rtw_rf.c | 88 +
drivers/staging/r8188eu/core/rtw_security.c | 1757 ++++
drivers/staging/r8188eu/core/rtw_sreset.c | 79 +
drivers/staging/r8188eu/core/rtw_sta_mgt.c | 609 ++
drivers/staging/r8188eu/core/rtw_wlan_util.c | 1690 ++++
drivers/staging/r8188eu/core/rtw_xmit.c | 2370 +++++
23 files changed, 36842 insertions(+)
create mode 100644 drivers/staging/r8188eu/core/rtw_ap.c
create mode 100644 drivers/staging/r8188eu/core/rtw_br_ext.c
create mode 100644 drivers/staging/r8188eu/core/rtw_cmd.c
create mode 100644 drivers/staging/r8188eu/core/rtw_debug.c
create mode 100644 drivers/staging/r8188eu/core/rtw_efuse.c
create mode 100644 drivers/staging/r8188eu/core/rtw_ieee80211.c
create mode 100644 drivers/staging/r8188eu/core/rtw_io.c
create mode 100644 drivers/staging/r8188eu/core/rtw_ioctl_set.c
create mode 100644 drivers/staging/r8188eu/core/rtw_iol.c
create mode 100644 drivers/staging/r8188eu/core/rtw_led.c
create mode 100644 drivers/staging/r8188eu/core/rtw_mlme.c
create mode 100644 drivers/staging/r8188eu/core/rtw_mlme_ext.c
create mode 100644 drivers/staging/r8188eu/core/rtw_mp.c
create mode 100644 drivers/staging/r8188eu/core/rtw_mp_ioctl.c
create mode 100644 drivers/staging/r8188eu/core/rtw_p2p.c
create mode 100644 drivers/staging/r8188eu/core/rtw_pwrctrl.c
create mode 100644 drivers/staging/r8188eu/core/rtw_recv.c
create mode 100644 drivers/staging/r8188eu/core/rtw_rf.c
create mode 100644 drivers/staging/r8188eu/core/rtw_security.c
create mode 100644 drivers/staging/r8188eu/core/rtw_sreset.c
create mode 100644 drivers/staging/r8188eu/core/rtw_sta_mgt.c
create mode 100644 drivers/staging/r8188eu/core/rtw_wlan_util.c
create mode 100644 drivers/staging/r8188eu/core/rtw_xmit.c
diff --git a/drivers/staging/r8188eu/core/rtw_ap.c b/drivers/staging/r8188eu/core/rtw_ap.c
new file mode 100644
index 000000000000..ba475a4933ec
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ap.c
@@ -0,0 +1,1976 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_AP_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <ieee80211.h>
+
+#ifdef CONFIG_88EU_AP_MODE
+
+void init_mlme_ap_info(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+ spin_lock_init(&pmlmepriv->bcn_update_lock);
+
+ /* for ACL */
+ _rtw_init_queue(&pacl_list->acl_node_q);
+
+ start_ap_mode(padapter);
+}
+
+void free_mlme_ap_info(struct adapter *padapter)
+{
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ pmlmepriv->update_bcn = false;
+ pmlmeext->bstart_bss = false;
+
+ rtw_sta_flush(padapter);
+
+ pmlmeinfo->state = _HW_STATE_NOLINK_;
+
+ /* free_assoc_sta_resources */
+ rtw_free_all_stainfo(padapter);
+
+ /* free bc/mc sta_info */
+ psta = rtw_get_bcmc_stainfo(padapter);
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(padapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+static void update_BCNTIM(struct adapter *padapter)
+{
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network);
+ unsigned char *pie = pnetwork_mlmeext->IEs;
+
+ /* update TIM IE */
+ if (true) {
+ u8 *p, *dst_ie, *premainder_ie = NULL;
+ u8 *pbackup_remainder_ie = NULL;
+ __le16 tim_bitmap_le;
+ uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
+
+ tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
+
+ p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+ if (p != NULL && tim_ielen > 0) {
+ tim_ielen += 2;
+ premainder_ie = p+tim_ielen;
+ tim_ie_offset = (int)(p - pie);
+ remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;
+ /* append TIM IE from dst_ie offset */
+ dst_ie = p;
+ } else {
+ tim_ielen = 0;
+
+ /* calculate head_len */
+ offset = _FIXED_IE_LENGTH_;
+ offset += pnetwork_mlmeext->Ssid.SsidLength + 2;
+
+ /* get supported rates len */
+ p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+ if (p != NULL)
+ offset += tmp_len+2;
+
+ /* DS Parameter Set IE, len = 3 */
+ offset += 3;
+
+ premainder_ie = pie + offset;
+
+ remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;
+
+ /* append TIM IE from offset */
+ dst_ie = pie + offset;
+ }
+
+ if (remainder_ielen > 0) {
+ pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+ if (pbackup_remainder_ie && premainder_ie)
+ memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+ }
+ *dst_ie++ = _TIM_IE_;
+
+ if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))
+ tim_ielen = 5;
+ else
+ tim_ielen = 4;
+
+ *dst_ie++ = tim_ielen;
+
+ *dst_ie++ = 0;/* DTIM count */
+ *dst_ie++ = 1;/* DTIM period */
+
+ if (pstapriv->tim_bitmap&BIT(0))/* for bc/mc frames */
+ *dst_ie++ = BIT(0);/* bitmap ctrl */
+ else
+ *dst_ie++ = 0;
+
+ if (tim_ielen == 4) {
+ *dst_ie++ = *(u8 *)&tim_bitmap_le;
+ } else if (tim_ielen == 5) {
+ memcpy(dst_ie, &tim_bitmap_le, 2);
+ dst_ie += 2;
+ }
+
+ /* copy remainder IE */
+ if (pbackup_remainder_ie) {
+ memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+ kfree(pbackup_remainder_ie);
+ }
+ offset = (uint)(dst_ie - pie);
+ pnetwork_mlmeext->IELength = offset + remainder_ielen;
+ }
+
+ set_tx_beacon_cmd(padapter);
+}
+
+void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8 index, u8 *data, u8 len)
+{
+ struct ndis_802_11_var_ie *pIE;
+ u8 bmatch = false;
+ u8 *pie = pnetwork->IEs;
+ u8 *p = NULL, *dst_ie = NULL, *premainder_ie = NULL;
+ u8 *pbackup_remainder_ie = NULL;
+ u32 i, offset, ielen = 0, ie_offset, remainder_ielen = 0;
+
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < pnetwork->IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(pnetwork->IEs + i);
+
+ if (pIE->ElementID > index) {
+ break;
+ } else if (pIE->ElementID == index) { /* already exist the same IE */
+ p = (u8 *)pIE;
+ ielen = pIE->Length;
+ bmatch = true;
+ break;
+ }
+ p = (u8 *)pIE;
+ ielen = pIE->Length;
+ i += (pIE->Length + 2);
+ }
+
+ if (p != NULL && ielen > 0) {
+ ielen += 2;
+
+ premainder_ie = p+ielen;
+
+ ie_offset = (int)(p - pie);
+
+ remainder_ielen = pnetwork->IELength - ie_offset - ielen;
+
+ if (bmatch)
+ dst_ie = p;
+ else
+ dst_ie = (p+ielen);
+ }
+
+ if (remainder_ielen > 0) {
+ pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+ if (pbackup_remainder_ie && premainder_ie)
+ memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+ }
+
+ *dst_ie++ = index;
+ *dst_ie++ = len;
+
+ memcpy(dst_ie, data, len);
+ dst_ie += len;
+
+ /* copy remainder IE */
+ if (pbackup_remainder_ie) {
+ memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+ kfree(pbackup_remainder_ie);
+ }
+
+ offset = (uint)(dst_ie - pie);
+ pnetwork->IELength = offset + remainder_ielen;
+}
+
+void rtw_remove_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8 index)
+{
+ u8 *p, *dst_ie = NULL, *premainder_ie = NULL;
+ u8 *pbackup_remainder_ie = NULL;
+ uint offset, ielen, ie_offset, remainder_ielen = 0;
+ u8 *pie = pnetwork->IEs;
+
+ p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, index, &ielen,
+ pnetwork->IELength - _FIXED_IE_LENGTH_);
+ if (p != NULL && ielen > 0) {
+ ielen += 2;
+
+ premainder_ie = p+ielen;
+
+ ie_offset = (int)(p - pie);
+
+ remainder_ielen = pnetwork->IELength - ie_offset - ielen;
+
+ dst_ie = p;
+ }
+
+ if (remainder_ielen > 0) {
+ pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+ if (pbackup_remainder_ie && premainder_ie)
+ memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+ }
+
+ /* copy remainder IE */
+ if (pbackup_remainder_ie) {
+ memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+ kfree(pbackup_remainder_ie);
+ }
+
+ offset = (uint)(dst_ie - pie);
+ pnetwork->IELength = offset + remainder_ielen;
+}
+
+static u8 chk_sta_is_alive(struct sta_info *psta)
+{
+ u8 ret = false;
+
+ if ((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) ==
+ (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts))
+ ;
+ else
+ ret = true;
+
+ sta_update_last_rx_pkts(psta);
+
+ return ret;
+}
+
+void expire_timeout_chk(struct adapter *padapter)
+{
+ struct list_head *phead, *plist;
+ u8 updated = 0;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 chk_alive_num = 0;
+ char chk_alive_list[NUM_STA];
+ int i;
+
+ spin_lock_bh(&pstapriv->auth_list_lock);
+
+ phead = &pstapriv->auth_list;
+ plist = phead->next;
+
+ /* check auth_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, auth_list);
+ plist = plist->next;
+
+ if (psta->expire_to > 0) {
+ psta->expire_to--;
+ if (psta->expire_to == 0) {
+ list_del_init(&psta->auth_list);
+ pstapriv->auth_list_cnt--;
+
+ DBG_88E("auth expire %6ph\n",
+ psta->hwaddr);
+
+ spin_unlock_bh(&pstapriv->auth_list_lock);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(padapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ spin_lock_bh(&pstapriv->auth_list_lock);
+ }
+ }
+
+ }
+ spin_unlock_bh(&pstapriv->auth_list_lock);
+
+ psta = NULL;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* check asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+ plist = plist->next;
+
+ if (chk_sta_is_alive(psta) || !psta->expire_to) {
+ psta->expire_to = pstapriv->expire_to;
+ psta->keep_alive_trycnt = 0;
+ psta->under_exist_checking = 0;
+ } else {
+ psta->expire_to--;
+ }
+
+ if (psta->expire_to <= 0) {
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ if (padapter->registrypriv.wifi_spec == 1) {
+ psta->expire_to = pstapriv->expire_to;
+ continue;
+ }
+
+ if (psta->state & WIFI_SLEEP_STATE) {
+ if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+ /* to check if alive by another methods if station is at ps mode. */
+ psta->expire_to = pstapriv->expire_to;
+ psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+ /* to update bcn with tim_bitmap for this station */
+ pstapriv->tim_bitmap |= BIT(psta->aid);
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+
+ if (!pmlmeext->active_keep_alive_check)
+ continue;
+ }
+ }
+ if (pmlmeext->active_keep_alive_check) {
+ int stainfo_offset;
+
+ stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+ if (stainfo_offset_valid(stainfo_offset))
+ chk_alive_list[chk_alive_num++] = stainfo_offset;
+ continue;
+ }
+
+ list_del_init(&psta->asoc_list);
+ pstapriv->asoc_list_cnt--;
+
+ DBG_88E("asoc expire %pM, state = 0x%x\n", (psta->hwaddr), psta->state);
+ updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+ } else {
+ /* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
+ if (psta->sleepq_len > (NR_XMITFRAME/pstapriv->asoc_list_cnt) &&
+ padapter->xmitpriv.free_xmitframe_cnt < (NR_XMITFRAME/pstapriv->asoc_list_cnt/2)) {
+ DBG_88E("%s sta:%pM, sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n", __func__,
+ (psta->hwaddr), psta->sleepq_len,
+ padapter->xmitpriv.free_xmitframe_cnt,
+ pstapriv->asoc_list_cnt);
+ wakeup_sta_to_xmit(padapter, psta);
+ }
+ }
+ }
+
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ if (chk_alive_num) {
+ u8 backup_oper_channel = 0;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ /* switch to correct channel of current network before issue keep-alive frames */
+ if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+ backup_oper_channel = rtw_get_oper_ch(padapter);
+ SelectChannel(padapter, pmlmeext->cur_channel);
+ }
+
+ /* issue null data to check sta alive*/
+ for (i = 0; i < chk_alive_num; i++) {
+ int ret = _FAIL;
+
+ psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+
+ if (psta->state & WIFI_SLEEP_STATE)
+ ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
+ else
+ ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+
+ psta->keep_alive_trycnt++;
+ if (ret == _SUCCESS) {
+ DBG_88E("asoc check, sta(%pM) is alive\n", (psta->hwaddr));
+ psta->expire_to = pstapriv->expire_to;
+ psta->keep_alive_trycnt = 0;
+ continue;
+ } else if (psta->keep_alive_trycnt <= 3) {
+ DBG_88E("ack check for asoc expire, keep_alive_trycnt =%d\n", psta->keep_alive_trycnt);
+ psta->expire_to = 1;
+ continue;
+ }
+
+ psta->keep_alive_trycnt = 0;
+
+ DBG_88E("asoc expire %pM, state = 0x%x\n", (psta->hwaddr), psta->state);
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ list_del_init(&psta->asoc_list);
+ pstapriv->asoc_list_cnt--;
+ updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+ }
+
+ if (backup_oper_channel > 0) /* back to the original operation channel */
+ SelectChannel(padapter, backup_oper_channel);
+ }
+
+ associated_clients_update(padapter, updated);
+}
+
+void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
+{
+ int i;
+ u8 rf_type;
+ u32 init_rate = 0;
+ unsigned char sta_band = 0, raid, shortGIrate = false;
+ unsigned char limit;
+ unsigned int tx_ra_bitmap = 0;
+ struct ht_priv *psta_ht = NULL;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+
+ if (psta)
+ psta_ht = &psta->htpriv;
+ else
+ return;
+
+ if (!(psta->state & _FW_LINKED))
+ return;
+
+ /* b/g mode ra_bitmap */
+ for (i = 0; i < sizeof(psta->bssrateset); i++) {
+ if (psta->bssrateset[i])
+ tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);
+ }
+ /* n mode ra_bitmap */
+ if (psta_ht->ht_option) {
+ rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+ if (rf_type == RF_2T2R)
+ limit = 16;/* 2R */
+ else
+ limit = 8;/* 1R */
+
+ for (i = 0; i < limit; i++) {
+ if (psta_ht->ht_cap.mcs.rx_mask[i/8] & BIT(i%8))
+ tx_ra_bitmap |= BIT(i+12);
+ }
+
+ /* max short GI rate */
+ shortGIrate = psta_ht->sgi;
+ }
+
+ if (pcur_network->Configuration.DSConfig > 14) {
+ /* 5G band */
+ if (tx_ra_bitmap & 0xffff000)
+ sta_band |= WIRELESS_11_5N | WIRELESS_11A;
+ else
+ sta_band |= WIRELESS_11A;
+ } else {
+ if (tx_ra_bitmap & 0xffff000)
+ sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;
+ else if (tx_ra_bitmap & 0xff0)
+ sta_band |= WIRELESS_11G | WIRELESS_11B;
+ else
+ sta_band |= WIRELESS_11B;
+ }
+
+ psta->wireless_mode = sta_band;
+
+ raid = networktype_to_raid(sta_band);
+ init_rate = get_highest_rate_idx(tx_ra_bitmap&0x0fffffff)&0x3f;
+
+ if (psta->aid < NUM_STA) {
+ u8 arg = 0;
+
+ arg = psta->mac_id&0x1f;
+
+ arg |= BIT(7);/* support entry 2~31 */
+
+ if (shortGIrate)
+ arg |= BIT(5);
+
+ tx_ra_bitmap |= ((raid<<28)&0xf0000000);
+
+ DBG_88E("%s => mac_id:%d , raid:%d , bitmap = 0x%x, arg = 0x%x\n",
+ __func__ , psta->mac_id, raid , tx_ra_bitmap, arg);
+
+ /* bitmap[0:27] = tx_rate_bitmap */
+ /* bitmap[28:31]= Rate Adaptive id */
+ /* arg[0:4] = macid */
+ /* arg[5] = Short GI */
+ rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level);
+
+ if (shortGIrate)
+ init_rate |= BIT(6);
+
+ /* set ra_id, init_rate */
+ psta->raid = raid;
+ psta->init_rate = init_rate;
+
+ } else {
+ DBG_88E("station aid %d exceed the max number\n", psta->aid);
+ }
+}
+
+void update_bmc_sta(struct adapter *padapter)
+{
+ u32 init_rate = 0;
+ unsigned char network_type, raid;
+ int i, supportRateNum = 0;
+ unsigned int tx_ra_bitmap = 0;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+ struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);
+
+ if (psta) {
+ psta->aid = 0;/* default set to 0 */
+ psta->mac_id = psta->aid + 1;
+
+ psta->qos_option = 0;
+ psta->htpriv.ht_option = false;
+
+ psta->ieee8021x_blocked = 0;
+
+ memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+ /* prepare for add_RATid */
+ supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates);
+ network_type = rtw_check_network_type((u8 *)&pcur_network->SupportedRates, supportRateNum, 1);
+
+ memcpy(psta->bssrateset, &pcur_network->SupportedRates, supportRateNum);
+ psta->bssratelen = supportRateNum;
+
+ /* b/g mode ra_bitmap */
+ for (i = 0; i < supportRateNum; i++) {
+ if (psta->bssrateset[i])
+ tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);
+ }
+
+ if (pcur_network->Configuration.DSConfig > 14) {
+ /* force to A mode. 5G doesn't support CCK rates */
+ network_type = WIRELESS_11A;
+ tx_ra_bitmap = 0x150; /* 6, 12, 24 Mbps */
+ } else {
+ /* force to b mode */
+ network_type = WIRELESS_11B;
+ tx_ra_bitmap = 0xf;
+ }
+
+ raid = networktype_to_raid(network_type);
+ init_rate = get_highest_rate_idx(tx_ra_bitmap&0x0fffffff)&0x3f;
+
+ /* ap mode */
+ rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+ {
+ u8 arg = 0;
+
+ arg = psta->mac_id&0x1f;
+ arg |= BIT(7);
+ tx_ra_bitmap |= ((raid<<28)&0xf0000000);
+ DBG_88E("update_bmc_sta, mask = 0x%x, arg = 0x%x\n", tx_ra_bitmap, arg);
+
+ /* bitmap[0:27] = tx_rate_bitmap */
+ /* bitmap[28:31]= Rate Adaptive id */
+ /* arg[0:4] = macid */
+ /* arg[5] = Short GI */
+ rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, 0);
+ }
+ /* set ra_id, init_rate */
+ psta->raid = raid;
+ psta->init_rate = init_rate;
+
+ rtw_sta_media_status_rpt(padapter, psta, 1);
+
+ spin_lock_bh(&psta->lock);
+ psta->state = _FW_LINKED;
+ spin_unlock_bh(&psta->lock);
+
+ } else {
+ DBG_88E("add_RATid_bmc_sta error!\n");
+ }
+}
+
+/* notes: */
+/* AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */
+/* MAC_ID = AID+1 for sta in ap/adhoc mode */
+/* MAC_ID = 1 for bc/mc for sta/ap/adhoc */
+/* MAC_ID = 0 for bssid for sta/ap/adhoc */
+/* CAM_ID = 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
+{
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+ struct ht_priv *phtpriv_sta = &psta->htpriv;
+ u16 sta_cap_info;
+ u16 ap_cap_info;
+
+ psta->mac_id = psta->aid+1;
+ DBG_88E("%s\n", __func__);
+
+ /* ap mode */
+ rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+ if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+ psta->ieee8021x_blocked = true;
+ else
+ psta->ieee8021x_blocked = false;
+
+ /* update sta's cap */
+
+ /* ERP */
+ VCS_update(padapter, psta);
+ /* HT related cap */
+ if (phtpriv_sta->ht_option) {
+ /* check if sta supports rx ampdu */
+ phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
+ sta_cap_info = le16_to_cpu(phtpriv_sta->ht_cap.cap_info);
+ ap_cap_info = le16_to_cpu(phtpriv_ap->ht_cap.cap_info);
+
+ /* check if sta support s Short GI */
+ if ((sta_cap_info & ap_cap_info) &
+ (IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40))
+ phtpriv_sta->sgi = true;
+
+ /* bwmode */
+ if ((sta_cap_info & ap_cap_info) & IEEE80211_HT_CAP_SUP_WIDTH) {
+ phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
+ phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+ }
+ psta->qos_option = true;
+ } else {
+ phtpriv_sta->ampdu_enable = false;
+ phtpriv_sta->sgi = false;
+ phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
+ phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ }
+
+ /* Rx AMPDU */
+ send_delba(padapter, 0, psta->hwaddr);/* recipient */
+
+ /* TX AMPDU */
+ send_delba(padapter, 1, psta->hwaddr);/* originator */
+ phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */
+ phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */
+
+ /* todo: init other variables */
+
+ memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+ spin_lock_bh(&psta->lock);
+ psta->state |= _FW_LINKED;
+ spin_unlock_bh(&psta->lock);
+}
+
+static void update_hw_ht_param(struct adapter *padapter)
+{
+ unsigned char max_AMPDU_len;
+ unsigned char min_MPDU_spacing;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ DBG_88E("%s\n", __func__);
+
+ /* handle A-MPDU parameter field */
+ /*
+ AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+ AMPDU_para [4:2]:Min MPDU Start Spacing
+ */
+ max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+ min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+
+ /* */
+ /* Config SM Power Save setting */
+ /* */
+ pmlmeinfo->SM_PS = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & 0x0C) >> 2;
+ if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+ DBG_88E("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+}
+
+static void start_bss_network(struct adapter *padapter, u8 *pbuf)
+{
+ u8 *p;
+ u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
+ u16 bcn_interval;
+ u32 acparm;
+ int ie_len;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct security_priv *psecuritypriv = &(padapter->securitypriv);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network);
+ struct HT_info_element *pht_info = NULL;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
+ cur_channel = pnetwork->Configuration.DSConfig;
+ cur_bwmode = HT_CHANNEL_WIDTH_20;
+ cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+ /* check if there is wps ie, */
+ /* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
+ /* and at first time the security ie (RSN/WPA IE) will not include in beacon. */
+ if (!rtw_get_wps_ie(pnetwork->IEs+_FIXED_IE_LENGTH_, pnetwork->IELength-_FIXED_IE_LENGTH_, NULL, NULL))
+ pmlmeext->bstart_bss = true;
+
+ /* todo: update wmm, ht cap */
+ if (pmlmepriv->qospriv.qos_option)
+ pmlmeinfo->WMM_enable = true;
+ if (pmlmepriv->htpriv.ht_option) {
+ pmlmeinfo->WMM_enable = true;
+ pmlmeinfo->HT_enable = true;
+
+ update_hw_ht_param(padapter);
+ }
+
+ if (pmlmepriv->cur_network.join_res != true) { /* setting only at first time */
+ /* WEP Key will be set before this function, do not clear CAM. */
+ if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) &&
+ (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
+ flush_all_cam_entry(padapter); /* clear CAM */
+ }
+
+ /* set MSR to AP_Mode */
+ Set_MSR(padapter, _HW_STATE_AP_);
+
+ /* Set BSSID REG */
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->MacAddress);
+
+ /* Set EDCA param reg */
+ acparm = 0x002F3217; /* VO */
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+ acparm = 0x005E4317; /* VI */
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+ acparm = 0x005ea42b;
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+ acparm = 0x0000A444; /* BK */
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+
+ /* Set Security */
+ val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+ rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+ /* Beacon Control related register */
+ rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));
+
+ UpdateBrateTbl(padapter, pnetwork->SupportedRates);
+ rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+
+ if (!pmlmepriv->cur_network.join_res) { /* setting only at first time */
+ /* turn on all dynamic functions */
+ Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+ }
+ /* set channel, bwmode */
+ p = rtw_get_ie((pnetwork->IEs + sizeof(struct ndis_802_11_fixed_ie)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - sizeof(struct ndis_802_11_fixed_ie)));
+ if (p && ie_len) {
+ pht_info = (struct HT_info_element *)(p+2);
+
+ if ((pregpriv->cbw40_enable) && (pht_info->infos[0] & BIT(2))) {
+ /* switch to the 40M Hz mode */
+ cur_bwmode = HT_CHANNEL_WIDTH_40;
+ switch (pht_info->infos[0] & 0x3) {
+ case 1:
+ cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+ break;
+ case 3:
+ cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+ break;
+ default:
+ cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ break;
+ }
+ }
+ }
+ /* TODO: need to judge the phy parameters on concurrent mode for single phy */
+ set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+
+ DBG_88E("CH =%d, BW =%d, offset =%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+
+ /* */
+ pmlmeext->cur_channel = cur_channel;
+ pmlmeext->cur_bwmode = cur_bwmode;
+ pmlmeext->cur_ch_offset = cur_ch_offset;
+ pmlmeext->cur_wireless_mode = pmlmepriv->cur_network.network_type;
+
+ /* update cur_wireless_mode */
+ update_wireless_mode(padapter);
+
+ /* udpate capability after cur_wireless_mode updated */
+ update_capinfo(padapter, rtw_get_capability((struct wlan_bssid_ex *)pnetwork));
+
+ /* let pnetwork_mlmeext == pnetwork_mlme. */
+ memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);
+
+#ifdef CONFIG_88EU_P2P
+ memcpy(pwdinfo->p2p_group_ssid, pnetwork->Ssid.Ssid, pnetwork->Ssid.SsidLength);
+ pwdinfo->p2p_group_ssid_len = pnetwork->Ssid.SsidLength;
+#endif /* CONFIG_88EU_P2P */
+
+ if (pmlmeext->bstart_bss) {
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+
+ /* issue beacon frame */
+ if (send_beacon(padapter) == _FAIL)
+ DBG_88E("issue_beacon, fail!\n");
+ }
+
+ /* update bc/mc sta_info */
+ update_bmc_sta(padapter);
+}
+
+int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf, int len)
+{
+ int ret = _SUCCESS;
+ u8 *p;
+ u8 *pHT_caps_ie = NULL;
+ u8 *pHT_info_ie = NULL;
+ struct sta_info *psta = NULL;
+ u16 cap, ht_cap = false;
+ uint ie_len = 0;
+ int group_cipher, pairwise_cipher;
+ u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
+ int supportRateNum = 0;
+ u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
+ u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct wlan_bssid_ex *pbss_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+ u8 *ie = pbss_network->IEs;
+
+ /* SSID */
+ /* Supported rates */
+ /* DS Params */
+ /* WLAN_EID_COUNTRY */
+ /* ERP Information element */
+ /* Extended supported rates */
+ /* WPA/WPA2 */
+ /* Wi-Fi Wireless Multimedia Extensions */
+ /* ht_capab, ht_oper */
+ /* WPS IE */
+
+ DBG_88E("%s, len =%d\n", __func__, len);
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+ return _FAIL;
+
+ if (len > MAX_IE_SZ)
+ return _FAIL;
+
+ pbss_network->IELength = len;
+
+ memset(ie, 0, MAX_IE_SZ);
+
+ memcpy(ie, pbuf, pbss_network->IELength);
+
+ if (pbss_network->InfrastructureMode != Ndis802_11APMode)
+ return _FAIL;
+
+ pbss_network->Rssi = 0;
+
+ memcpy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ /* beacon interval */
+ p = rtw_get_beacon_interval_from_ie(ie);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+ pbss_network->Configuration.BeaconPeriod = get_unaligned_le16(p);
+
+ /* capability */
+ cap = get_unaligned_le16(ie);
+
+ /* SSID */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0) {
+ memset(&pbss_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+ memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len);
+ pbss_network->Ssid.SsidLength = ie_len;
+ }
+
+ /* channel */
+ channel = 0;
+ pbss_network->Configuration.Length = 0;
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0)
+ channel = *(p + 2);
+
+ pbss_network->Configuration.DSConfig = channel;
+
+ memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
+ /* get supported rates */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p != NULL) {
+ memcpy(supportRate, p+2, ie_len);
+ supportRateNum = ie_len;
+ }
+
+ /* get ext_supported rates */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);
+ if (p != NULL) {
+ memcpy(supportRate+supportRateNum, p+2, ie_len);
+ supportRateNum += ie_len;
+ }
+
+ network_type = rtw_check_network_type(supportRate, supportRateNum, channel);
+
+ rtw_set_supported_rate(pbss_network->SupportedRates, network_type);
+
+ /* parsing ERP_IE */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0)
+ ERP_IE_handler(padapter, (struct ndis_802_11_var_ie *)p);
+
+ /* update privacy/security */
+ if (cap & BIT(4))
+ pbss_network->Privacy = 1;
+ else
+ pbss_network->Privacy = 0;
+
+ psecuritypriv->wpa_psk = 0;
+
+ /* wpa2 */
+ group_cipher = 0;
+ pairwise_cipher = 0;
+ psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
+ psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0) {
+ if (rtw_parse_wpa2_ie(p, ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+ psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+ psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */
+ psecuritypriv->wpa_psk |= BIT(1);
+
+ psecuritypriv->wpa2_group_cipher = group_cipher;
+ psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher;
+ }
+ }
+ /* wpa */
+ ie_len = 0;
+ group_cipher = 0;
+ pairwise_cipher = 0;
+ psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
+ psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
+ for (p = ie + _BEACON_IE_OFFSET_;; p += (ie_len + 2)) {
+ p = rtw_get_ie(p, _SSN_IE_1_, &ie_len,
+ (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+ if ((p) && (!memcmp(p+2, OUI1, 4))) {
+ if (rtw_parse_wpa_ie(p, ie_len+2, &group_cipher,
+ &pairwise_cipher, NULL) == _SUCCESS) {
+ psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+ psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */
+
+ psecuritypriv->wpa_psk |= BIT(0);
+
+ psecuritypriv->wpa_group_cipher = group_cipher;
+ psecuritypriv->wpa_pairwise_cipher = pairwise_cipher;
+ }
+ break;
+ }
+ if ((p == NULL) || (ie_len == 0))
+ break;
+ }
+
+ /* wmm */
+ ie_len = 0;
+ pmlmepriv->qospriv.qos_option = 0;
+ if (pregistrypriv->wmm_enable) {
+ for (p = ie + _BEACON_IE_OFFSET_;; p += (ie_len + 2)) {
+ p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len,
+ (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+ if ((p) && !memcmp(p+2, WMM_PARA_IE, 6)) {
+ pmlmepriv->qospriv.qos_option = 1;
+
+ *(p+8) |= BIT(7);/* QoS Info, support U-APSD */
+
+ /* disable all ACM bits since the WMM admission control is not supported */
+ *(p + 10) &= ~BIT(4); /* BE */
+ *(p + 14) &= ~BIT(4); /* BK */
+ *(p + 18) &= ~BIT(4); /* VI */
+ *(p + 22) &= ~BIT(4); /* VO */
+ break;
+ }
+
+ if ((p == NULL) || (ie_len == 0))
+ break;
+ }
+ }
+ /* parsing HT_CAP_IE */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len,
+ (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0) {
+ u8 rf_type;
+ struct ieee80211_ht_cap *pht_cap = (struct ieee80211_ht_cap *)(p+2);
+
+ pHT_caps_ie = p;
+ ht_cap = true;
+ network_type |= WIRELESS_11_24N;
+
+ rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+ if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
+ (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP))
+ pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));
+ else
+ pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);
+
+ /* set Max Rx AMPDU size to 64K */
+ pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR & 0x03);
+
+ if(rf_type == RF_1T1R) {
+ pht_cap->mcs.rx_mask[0] = 0xff;
+ pht_cap->mcs.rx_mask[1] = 0x0;
+ }
+ memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len);
+ }
+
+ /* parsing HT_INFO_IE */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len,
+ (pbss_network->IELength - _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0)
+ pHT_info_ie = p;
+ switch (network_type) {
+ case WIRELESS_11B:
+ pbss_network->NetworkTypeInUse = Ndis802_11DS;
+ break;
+ case WIRELESS_11G:
+ case WIRELESS_11BG:
+ case WIRELESS_11G_24N:
+ case WIRELESS_11BG_24N:
+ pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
+ break;
+ case WIRELESS_11A:
+ pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;
+ break;
+ default:
+ pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
+ break;
+ }
+
+ pmlmepriv->cur_network.network_type = network_type;
+
+ pmlmepriv->htpriv.ht_option = false;
+
+ if ((psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) ||
+ (psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
+ /* todo: */
+ /* ht_cap = false; */
+ }
+
+ /* ht_cap */
+ if (pregistrypriv->ht_enable && ht_cap) {
+ pmlmepriv->htpriv.ht_option = true;
+ pmlmepriv->qospriv.qos_option = 1;
+
+ if (pregistrypriv->ampdu_enable == 1)
+ pmlmepriv->htpriv.ampdu_enable = true;
+ HT_caps_handler(padapter, (struct ndis_802_11_var_ie *)pHT_caps_ie);
+
+ HT_info_handler(padapter, (struct ndis_802_11_var_ie *)pHT_info_ie);
+ }
+
+ pbss_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pbss_network);
+
+ /* issue beacon to start bss network */
+ start_bss_network(padapter, (u8 *)pbss_network);
+
+ /* alloc sta_info for ap itself */
+ psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+ if (!psta) {
+ psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+ if (psta == NULL)
+ return _FAIL;
+ }
+
+ /* fix bug of flush_cam_entry at STOP AP mode */
+ psta->state |= WIFI_AP_STATE;
+ rtw_indicate_connect(padapter);
+ pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */
+ return ret;
+}
+
+void rtw_set_macaddr_acl(struct adapter *padapter, int mode)
+{
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+ DBG_88E("%s, mode =%d\n", __func__, mode);
+
+ pacl_list->mode = mode;
+}
+
+int rtw_acl_add_sta(struct adapter *padapter, u8 *addr)
+{
+ struct list_head *plist, *phead;
+ u8 added = false;
+ int i, ret = 0;
+ struct rtw_wlan_acl_node *paclnode;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+ struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+ DBG_88E("%s(acl_num =%d) =%pM\n", __func__, pacl_list->num, (addr));
+
+ if ((NUM_ACL-1) < pacl_list->num)
+ return -1;
+
+ spin_lock_bh(&pacl_node_q->lock);
+
+ phead = get_list_head(pacl_node_q);
+ plist = phead->next;
+
+ while (phead != plist) {
+ paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+ plist = plist->next;
+
+ if (!memcmp(paclnode->addr, addr, ETH_ALEN)) {
+ if (paclnode->valid) {
+ added = true;
+ DBG_88E("%s, sta has been added\n", __func__);
+ break;
+ }
+ }
+ }
+
+ spin_unlock_bh(&pacl_node_q->lock);
+
+ if (added)
+ return ret;
+
+ spin_lock_bh(&pacl_node_q->lock);
+
+ for (i = 0; i < NUM_ACL; i++) {
+ paclnode = &pacl_list->aclnode[i];
+
+ if (!paclnode->valid) {
+ INIT_LIST_HEAD(&paclnode->list);
+
+ memcpy(paclnode->addr, addr, ETH_ALEN);
+
+ paclnode->valid = true;
+
+ list_add_tail(&paclnode->list, get_list_head(pacl_node_q));
+
+ pacl_list->num++;
+
+ break;
+ }
+ }
+
+ DBG_88E("%s, acl_num =%d\n", __func__, pacl_list->num);
+
+ spin_unlock_bh(&pacl_node_q->lock);
+
+ return ret;
+}
+
+int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
+{
+ struct list_head *plist, *phead;
+ int ret = 0;
+ struct rtw_wlan_acl_node *paclnode;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+ struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+ DBG_88E("%s(acl_num =%d) =%pM\n", __func__, pacl_list->num, (addr));
+
+ spin_lock_bh(&pacl_node_q->lock);
+
+ phead = get_list_head(pacl_node_q);
+ plist = phead->next;
+
+ while (phead != plist) {
+ paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+ plist = plist->next;
+
+ if (!memcmp(paclnode->addr, addr, ETH_ALEN)) {
+ if (paclnode->valid) {
+ paclnode->valid = false;
+
+ list_del_init(&paclnode->list);
+
+ pacl_list->num--;
+ }
+ }
+ }
+
+ spin_unlock_bh(&pacl_node_q->lock);
+
+ DBG_88E("%s, acl_num =%d\n", __func__, pacl_list->num);
+ return ret;
+}
+
+static void update_bcn_fixed_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_erpinfo_ie(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+ unsigned char *p, *ie = pnetwork->IEs;
+ u32 len = 0;
+
+ DBG_88E("%s, ERP_enable =%d\n", __func__, pmlmeinfo->ERP_enable);
+
+ if (!pmlmeinfo->ERP_enable)
+ return;
+
+ /* parsing ERP_IE */
+ p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len,
+ (pnetwork->IELength - _BEACON_IE_OFFSET_));
+ if (p && len > 0) {
+ struct ndis_802_11_var_ie *pIE = (struct ndis_802_11_var_ie *)p;
+
+ if (pmlmepriv->num_sta_non_erp == 1)
+ pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION;
+ else
+ pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION);
+
+ if (pmlmepriv->num_sta_no_short_preamble > 0)
+ pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;
+ else
+ pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE);
+
+ ERP_IE_handler(padapter, pIE);
+ }
+}
+
+static void update_bcn_htcap_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_htinfo_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_rsn_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_wpa_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_wmm_ie(struct adapter *padapter)
+{
+ DBG_88E("%s\n", __func__);
+}
+
+static void update_bcn_wps_ie(struct adapter *padapter)
+{
+ u8 *pwps_ie = NULL, *pwps_ie_src;
+ u8 *premainder_ie, *pbackup_remainder_ie = NULL;
+ uint wps_ielen = 0, wps_offset, remainder_ielen;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+ unsigned char *ie = pnetwork->IEs;
+ u32 ielen = pnetwork->IELength;
+
+ DBG_88E("%s\n", __func__);
+
+ pwps_ie = rtw_get_wps_ie(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+ if (pwps_ie == NULL || wps_ielen == 0)
+ return;
+
+ wps_offset = (uint)(pwps_ie-ie);
+
+ premainder_ie = pwps_ie + wps_ielen;
+
+ remainder_ielen = ielen - wps_offset - wps_ielen;
+
+ if (remainder_ielen > 0) {
+ pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+ if (pbackup_remainder_ie)
+ memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+ }
+
+ pwps_ie_src = pmlmepriv->wps_beacon_ie;
+ if (pwps_ie_src == NULL)
+ return;
+
+ wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
+ if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
+ memcpy(pwps_ie, pwps_ie_src, wps_ielen+2);
+ pwps_ie += (wps_ielen+2);
+
+ if (pbackup_remainder_ie)
+ memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);
+
+ /* update IELength */
+ pnetwork->IELength = wps_offset + (wps_ielen+2) + remainder_ielen;
+ }
+
+ if (pbackup_remainder_ie)
+ kfree(pbackup_remainder_ie);
+}
+
+static void update_bcn_p2p_ie(struct adapter *padapter)
+{
+}
+
+static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
+{
+ DBG_88E("%s\n", __func__);
+
+ if (!memcmp(RTW_WPA_OUI, oui, 4))
+ update_bcn_wpa_ie(padapter);
+ else if (!memcmp(WMM_OUI, oui, 4))
+ update_bcn_wmm_ie(padapter);
+ else if (!memcmp(WPS_OUI, oui, 4))
+ update_bcn_wps_ie(padapter);
+ else if (!memcmp(P2P_OUI, oui, 4))
+ update_bcn_p2p_ie(padapter);
+ else
+ DBG_88E("unknown OUI type!\n");
+}
+
+void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
+{
+ struct mlme_priv *pmlmepriv;
+ struct mlme_ext_priv *pmlmeext;
+
+ if (!padapter)
+ return;
+
+ pmlmepriv = &(padapter->mlmepriv);
+ pmlmeext = &(padapter->mlmeextpriv);
+
+ if (!pmlmeext->bstart_bss)
+ return;
+
+ spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+ switch (ie_id) {
+ case 0xFF:
+ update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+ break;
+ case _TIM_IE_:
+ update_BCNTIM(padapter);
+ break;
+ case _ERPINFO_IE_:
+ update_bcn_erpinfo_ie(padapter);
+ break;
+ case _HT_CAPABILITY_IE_:
+ update_bcn_htcap_ie(padapter);
+ break;
+ case _RSN_IE_2_:
+ update_bcn_rsn_ie(padapter);
+ break;
+ case _HT_ADD_INFO_IE_:
+ update_bcn_htinfo_ie(padapter);
+ break;
+ case _VENDOR_SPECIFIC_IE_:
+ update_bcn_vendor_spec_ie(padapter, oui);
+ break;
+ default:
+ break;
+ }
+
+ pmlmepriv->update_bcn = true;
+
+ spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+ if (tx)
+ set_tx_beacon_cmd(padapter);
+}
+
+/*
+op_mode
+Set to 0 (HT pure) under the followign conditions
+ - all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+ - all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+Set to 1 (HT non-member protection) if there may be non-HT STAs
+ in both the primary and the secondary channel
+Set to 2 if only HT STAs are associated in BSS,
+ however and at least one 20 MHz HT STA is associated
+Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+ (currently non-GF HT station is considered as non-HT STA also)
+*/
+static int rtw_ht_operation_update(struct adapter *padapter)
+{
+ u16 cur_op_mode, new_op_mode;
+ int op_mode_changes = 0;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+
+ if (pmlmepriv->htpriv.ht_option)
+ return 0;
+
+ DBG_88E("%s current operation mode = 0x%X\n",
+ __func__, pmlmepriv->ht_op_mode);
+
+ if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+ pmlmepriv->num_sta_ht_no_gf) {
+ pmlmepriv->ht_op_mode |=
+ HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+ op_mode_changes++;
+ } else if ((pmlmepriv->ht_op_mode &
+ HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+ pmlmepriv->num_sta_ht_no_gf == 0) {
+ pmlmepriv->ht_op_mode &=
+ ~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+ op_mode_changes++;
+ }
+
+ if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+ (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {
+ pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+ op_mode_changes++;
+ } else if ((pmlmepriv->ht_op_mode &
+ HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+ (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
+ pmlmepriv->ht_op_mode &=
+ ~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+ op_mode_changes++;
+ }
+
+ /* Note: currently we switch to the MIXED op mode if HT non-greenfield
+ * station is associated. Probably it's a theoretical case, since
+ * it looks like all known HT STAs support greenfield.
+ */
+ new_op_mode = 0;
+ if (pmlmepriv->num_sta_no_ht ||
+ (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))
+ new_op_mode = OP_MODE_MIXED;
+ else if ((le16_to_cpu(phtpriv_ap->ht_cap.cap_info) &
+ IEEE80211_HT_CAP_SUP_WIDTH) &&
+ pmlmepriv->num_sta_ht_20mhz)
+ new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+ else if (pmlmepriv->olbc_ht)
+ new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+ else
+ new_op_mode = OP_MODE_PURE;
+
+ cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+ if (cur_op_mode != new_op_mode) {
+ pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+ pmlmepriv->ht_op_mode |= new_op_mode;
+ op_mode_changes++;
+ }
+
+ DBG_88E("%s new operation mode = 0x%X changes =%d\n",
+ __func__, pmlmepriv->ht_op_mode, op_mode_changes);
+
+ return op_mode_changes;
+}
+
+void associated_clients_update(struct adapter *padapter, u8 updated)
+{
+ /* update associcated stations cap. */
+ if (updated) {
+ struct list_head *phead, *plist;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* check asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+
+ plist = plist->next;
+
+ VCS_update(padapter, psta);
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+ }
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta)
+{
+ u8 beacon_updated = false;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) {
+ if (!psta->no_short_preamble_set) {
+ psta->no_short_preamble_set = 1;
+
+ pmlmepriv->num_sta_no_short_preamble++;
+
+ if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+ (pmlmepriv->num_sta_no_short_preamble == 1)) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+ } else {
+ if (psta->no_short_preamble_set) {
+ psta->no_short_preamble_set = 0;
+
+ pmlmepriv->num_sta_no_short_preamble--;
+
+ if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+ (pmlmepriv->num_sta_no_short_preamble == 0)) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+ }
+
+ if (psta->flags & WLAN_STA_NONERP) {
+ if (!psta->nonerp_set) {
+ psta->nonerp_set = 1;
+
+ pmlmepriv->num_sta_non_erp++;
+
+ if (pmlmepriv->num_sta_non_erp == 1) {
+ beacon_updated = true;
+ update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+ }
+ }
+ } else {
+ if (psta->nonerp_set) {
+ psta->nonerp_set = 0;
+
+ pmlmepriv->num_sta_non_erp--;
+
+ if (pmlmepriv->num_sta_non_erp == 0) {
+ beacon_updated = true;
+ update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+ }
+ }
+ }
+
+ if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT)) {
+ if (!psta->no_short_slot_time_set) {
+ psta->no_short_slot_time_set = 1;
+
+ pmlmepriv->num_sta_no_short_slot_time++;
+
+ if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+ (pmlmepriv->num_sta_no_short_slot_time == 1)) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+ } else {
+ if (psta->no_short_slot_time_set) {
+ psta->no_short_slot_time_set = 0;
+
+ pmlmepriv->num_sta_no_short_slot_time--;
+
+ if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+ (pmlmepriv->num_sta_no_short_slot_time == 0)) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+ }
+
+ if (psta->flags & WLAN_STA_HT) {
+ u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
+
+ DBG_88E("HT: STA %pM HT Capabilities Info: 0x%04x\n",
+ (psta->hwaddr), ht_capab);
+
+ if (psta->no_ht_set) {
+ psta->no_ht_set = 0;
+ pmlmepriv->num_sta_no_ht--;
+ }
+
+ if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {
+ if (!psta->no_ht_gf_set) {
+ psta->no_ht_gf_set = 1;
+ pmlmepriv->num_sta_ht_no_gf++;
+ }
+ DBG_88E("%s STA %pM - no greenfield, num of non-gf stations %d\n",
+ __func__, (psta->hwaddr),
+ pmlmepriv->num_sta_ht_no_gf);
+ }
+
+ if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH) == 0) {
+ if (!psta->ht_20mhz_set) {
+ psta->ht_20mhz_set = 1;
+ pmlmepriv->num_sta_ht_20mhz++;
+ }
+ DBG_88E("%s STA %pM - 20 MHz HT, num of 20MHz HT STAs %d\n",
+ __func__, (psta->hwaddr),
+ pmlmepriv->num_sta_ht_20mhz);
+ }
+ } else {
+ if (!psta->no_ht_set) {
+ psta->no_ht_set = 1;
+ pmlmepriv->num_sta_no_ht++;
+ }
+ if (pmlmepriv->htpriv.ht_option) {
+ DBG_88E("%s STA %pM - no HT, num of non-HT stations %d\n",
+ __func__, (psta->hwaddr),
+ pmlmepriv->num_sta_no_ht);
+ }
+ }
+
+ if (rtw_ht_operation_update(padapter) > 0) {
+ update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+ update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+ }
+
+ /* update associcated stations cap. */
+ associated_clients_update(padapter, beacon_updated);
+
+ DBG_88E("%s, updated =%d\n", __func__, beacon_updated);
+}
+
+u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta)
+{
+ u8 beacon_updated = false;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ if (!psta)
+ return beacon_updated;
+
+ if (psta->no_short_preamble_set) {
+ psta->no_short_preamble_set = 0;
+ pmlmepriv->num_sta_no_short_preamble--;
+ if (pmlmeext->cur_wireless_mode > WIRELESS_11B &&
+ pmlmepriv->num_sta_no_short_preamble == 0) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+
+ if (psta->nonerp_set) {
+ psta->nonerp_set = 0;
+ pmlmepriv->num_sta_non_erp--;
+ if (pmlmepriv->num_sta_non_erp == 0) {
+ beacon_updated = true;
+ update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+ }
+ }
+
+ if (psta->no_short_slot_time_set) {
+ psta->no_short_slot_time_set = 0;
+ pmlmepriv->num_sta_no_short_slot_time--;
+ if (pmlmeext->cur_wireless_mode > WIRELESS_11B &&
+ pmlmepriv->num_sta_no_short_slot_time == 0) {
+ beacon_updated = true;
+ update_beacon(padapter, 0xFF, NULL, true);
+ }
+ }
+
+ if (psta->no_ht_gf_set) {
+ psta->no_ht_gf_set = 0;
+ pmlmepriv->num_sta_ht_no_gf--;
+ }
+
+ if (psta->no_ht_set) {
+ psta->no_ht_set = 0;
+ pmlmepriv->num_sta_no_ht--;
+ }
+
+ if (psta->ht_20mhz_set) {
+ psta->ht_20mhz_set = 0;
+ pmlmepriv->num_sta_ht_20mhz--;
+ }
+
+ if (rtw_ht_operation_update(padapter) > 0) {
+ update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+ update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+ }
+
+ /* update associcated stations cap. */
+
+ DBG_88E("%s, updated =%d\n", __func__, beacon_updated);
+
+ return beacon_updated;
+}
+
+u8 ap_free_sta(struct adapter *padapter, struct sta_info *psta,
+ bool active, u16 reason)
+{
+ u8 beacon_updated = false;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ if (!psta)
+ return beacon_updated;
+
+ /* tear down Rx AMPDU */
+ send_delba(padapter, 0, psta->hwaddr);/* recipient */
+
+ /* tear down TX AMPDU */
+ send_delba(padapter, 1, psta->hwaddr);/* originator */
+ psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+ psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+ if (active)
+ issue_deauth(padapter, psta->hwaddr, reason);
+
+ /* clear cam entry / key */
+ rtw_clearstakey_cmd(padapter, (u8 *)psta, (u8)(psta->mac_id + 3), true);
+
+ spin_lock_bh(&psta->lock);
+ psta->state &= ~_FW_LINKED;
+ spin_unlock_bh(&psta->lock);
+
+ rtw_indicate_sta_disassoc_event(padapter, psta);
+
+ report_del_sta_event(padapter, psta->hwaddr, reason);
+
+ beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(padapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ return beacon_updated;
+}
+
+int rtw_ap_inform_ch_switch(struct adapter *padapter, u8 new_ch, u8 ch_offset)
+{
+ struct list_head *phead, *plist;
+ int ret = 0;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ return ret;
+
+ DBG_88E(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",
+ FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* for each sta in asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+ plist = plist->next;
+
+ issue_action_spct_ch_switch(padapter, psta->hwaddr, new_ch, ch_offset);
+ psta->expire_to = ((pstapriv->expire_to * 2) > 5) ? 5 : (pstapriv->expire_to * 2);
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ issue_action_spct_ch_switch(padapter, bc_addr, new_ch, ch_offset);
+
+ return ret;
+}
+
+int rtw_sta_flush(struct adapter *padapter)
+{
+ struct list_head *phead, *plist;
+ int ret = 0;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+ DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ return ret;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* free sta asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+
+ plist = plist->next;
+
+ list_del_init(&psta->asoc_list);
+ pstapriv->asoc_list_cnt--;
+
+ ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
+
+ associated_clients_update(padapter, true);
+
+ return ret;
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void sta_info_update(struct adapter *padapter, struct sta_info *psta)
+{
+ int flags = psta->flags;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ /* update wmm cap. */
+ if (WLAN_STA_WME&flags)
+ psta->qos_option = 1;
+ else
+ psta->qos_option = 0;
+
+ if (pmlmepriv->qospriv.qos_option == 0)
+ psta->qos_option = 0;
+
+ /* update 802.11n ht cap. */
+ if (WLAN_STA_HT&flags) {
+ psta->htpriv.ht_option = true;
+ psta->qos_option = 1;
+ } else {
+ psta->htpriv.ht_option = false;
+ }
+
+ if (!pmlmepriv->htpriv.ht_option)
+ psta->htpriv.ht_option = false;
+
+ update_sta_info_apmode(padapter, psta);
+}
+
+/* called >= TSR LEVEL for USB or SDIO Interface*/
+void ap_sta_info_defer_update(struct adapter *padapter, struct sta_info *psta)
+{
+ if (psta->state & _FW_LINKED) {
+ /* add ratid */
+ add_RATid(padapter, psta, 0);/* DM_RATR_STA_INIT */
+ }
+}
+
+void start_ap_mode(struct adapter *padapter)
+{
+ int i;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+ pmlmepriv->update_bcn = false;
+
+ pmlmeext->bstart_bss = false;
+
+ pmlmepriv->num_sta_non_erp = 0;
+
+ pmlmepriv->num_sta_no_short_slot_time = 0;
+
+ pmlmepriv->num_sta_no_short_preamble = 0;
+
+ pmlmepriv->num_sta_ht_no_gf = 0;
+ pmlmepriv->num_sta_no_ht = 0;
+ pmlmepriv->num_sta_ht_20mhz = 0;
+
+ pmlmepriv->olbc = false;
+
+ pmlmepriv->olbc_ht = false;
+
+ pmlmepriv->ht_op_mode = 0;
+
+ for (i = 0; i < NUM_STA; i++)
+ pstapriv->sta_aid[i] = NULL;
+
+ pmlmepriv->wps_beacon_ie = NULL;
+ pmlmepriv->wps_probe_resp_ie = NULL;
+ pmlmepriv->wps_assoc_resp_ie = NULL;
+
+ pmlmepriv->p2p_beacon_ie = NULL;
+ pmlmepriv->p2p_probe_resp_ie = NULL;
+
+ /* for ACL */
+ INIT_LIST_HEAD(&(pacl_list->acl_node_q.queue));
+ pacl_list->num = 0;
+ pacl_list->mode = 0;
+ for (i = 0; i < NUM_ACL; i++) {
+ INIT_LIST_HEAD(&pacl_list->aclnode[i].list);
+ pacl_list->aclnode[i].valid = false;
+ }
+}
+
+void stop_ap_mode(struct adapter *padapter)
+{
+ struct list_head *phead, *plist;
+ struct rtw_wlan_acl_node *paclnode;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+ struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+ pmlmepriv->update_bcn = false;
+ pmlmeext->bstart_bss = false;
+
+ /* reset and init security priv , this can refine with rtw_reset_securitypriv */
+ memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv));
+ padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+ padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+
+ /* for ACL */
+ spin_lock_bh(&pacl_node_q->lock);
+ phead = get_list_head(pacl_node_q);
+ plist = phead->next;
+ while (phead != plist) {
+ paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+ plist = plist->next;
+
+ if (paclnode->valid) {
+ paclnode->valid = false;
+
+ list_del_init(&paclnode->list);
+
+ pacl_list->num--;
+ }
+ }
+ spin_unlock_bh(&pacl_node_q->lock);
+
+ DBG_88E("%s, free acl_node_queue, num =%d\n", __func__, pacl_list->num);
+
+ rtw_sta_flush(padapter);
+
+ /* free_assoc_sta_resources */
+ rtw_free_all_stainfo(padapter);
+
+ psta = rtw_get_bcmc_stainfo(padapter);
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(padapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ rtw_init_bcmc_stainfo(padapter);
+
+ rtw_free_mlme_priv_ie_data(pmlmepriv);
+}
+
+#endif /* CONFIG_88EU_AP_MODE */
diff --git a/drivers/staging/r8188eu/core/rtw_br_ext.c b/drivers/staging/r8188eu/core/rtw_br_ext.c
new file mode 100644
index 000000000000..1236f50dc22f
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_br_ext.c
@@ -0,0 +1,1184 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_BR_EXT_C_
+
+#include <linux/if_arp.h>
+#include <net/ip.h>
+#include <net/ipx.h>
+#include <linux/atalk.h>
+#include <linux/udp.h>
+#include <linux/if_pppox.h>
+
+#include <drv_types.h>
+#include "rtw_br_ext.h"
+#include <usb_osintf.h>
+#include <recv_osdep.h>
+
+#ifndef csum_ipv6_magic
+#include <net/ip6_checksum.h>
+#endif
+
+#include <linux/ipv6.h>
+#include <linux/icmpv6.h>
+#include <net/ndisc.h>
+#include <net/checksum.h>
+
+#define NAT25_IPV4 01
+#define NAT25_IPV6 02
+#define NAT25_IPX 03
+#define NAT25_APPLE 04
+#define NAT25_PPPOE 05
+
+#define RTL_RELAY_TAG_LEN (ETH_ALEN)
+#define TAG_HDR_LEN 4
+
+#define MAGIC_CODE 0x8186
+#define MAGIC_CODE_LEN 2
+#define WAIT_TIME_PPPOE 5 /* waiting time for pppoe server in sec */
+
+/*-----------------------------------------------------------------
+ How database records network address:
+ 0 1 2 3 4 5 6 7 8 9 10
+ |----|----|----|----|----|----|----|----|----|----|----|
+ IPv4 |type| | IP addr |
+ IPX |type| Net addr | Node addr |
+ IPX |type| Net addr |Sckt addr|
+ Apple |type| Network |node|
+ PPPoE |type| SID | AC MAC |
+-----------------------------------------------------------------*/
+
+/* Find a tag in pppoe frame and return the pointer */
+static inline unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type)
+{
+ unsigned char *cur_ptr, *start_ptr;
+ unsigned short tagLen, tagType;
+
+ start_ptr = cur_ptr = (unsigned char *)ph->tag;
+ while ((cur_ptr - start_ptr) < ntohs(ph->length)) {
+ /* prevent un-alignment access */
+ tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]);
+ tagLen = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]);
+ if (tagType == type)
+ return cur_ptr;
+ cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen;
+ }
+ return NULL;
+}
+
+static inline int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag)
+{
+ struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+ int data_len;
+
+ data_len = tag->tag_len + TAG_HDR_LEN;
+ if (skb_tailroom(skb) < data_len) {
+ _DEBUG_ERR("skb_tailroom() failed in add SID tag!\n");
+ return -1;
+ }
+
+ skb_put(skb, data_len);
+ /* have a room for new tag */
+ memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length));
+ ph->length = htons(ntohs(ph->length) + data_len);
+ memcpy((unsigned char *)ph->tag, tag, data_len);
+ return data_len;
+}
+
+static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len)
+{
+ int tail_len;
+ unsigned long end, tail;
+
+ if ((src+len) > skb_tail_pointer(skb) || skb->len < len)
+ return -1;
+
+ tail = (unsigned long)skb_tail_pointer(skb);
+ end = (unsigned long)src+len;
+ if (tail < end)
+ return -1;
+
+ tail_len = (int)(tail-end);
+ if (tail_len > 0)
+ memmove(src, src+len, tail_len);
+
+ skb_trim(skb, skb->len-len);
+ return 0;
+}
+
+static inline unsigned long __nat25_timeout(struct adapter *priv)
+{
+ unsigned long timeout;
+
+ timeout = jiffies - NAT25_AGEING_TIME*HZ;
+
+ return timeout;
+}
+
+static inline int __nat25_has_expired(struct adapter *priv,
+ struct nat25_network_db_entry *fdb)
+{
+ if (time_before_eq(fdb->ageing_timer, __nat25_timeout(priv)))
+ return 1;
+
+ return 0;
+}
+
+static inline void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr,
+ unsigned int *ipAddr)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_IPV4;
+ memcpy(networkAddr+7, (unsigned char *)ipAddr, 4);
+}
+
+static inline void __nat25_generate_ipx_network_addr_with_node(unsigned char *networkAddr,
+ unsigned int *ipxNetAddr, unsigned char *ipxNodeAddr)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_IPX;
+ memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
+ memcpy(networkAddr+5, ipxNodeAddr, 6);
+}
+
+static inline void __nat25_generate_ipx_network_addr_with_socket(unsigned char *networkAddr,
+ unsigned int *ipxNetAddr, unsigned short *ipxSocketAddr)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_IPX;
+ memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
+ memcpy(networkAddr+5, (unsigned char *)ipxSocketAddr, 2);
+}
+
+static inline void __nat25_generate_apple_network_addr(unsigned char *networkAddr,
+ unsigned short *network, unsigned char *node)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_APPLE;
+ memcpy(networkAddr+1, (unsigned char *)network, 2);
+ networkAddr[3] = *node;
+}
+
+static inline void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr,
+ unsigned char *ac_mac, unsigned short *sid)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_PPPOE;
+ memcpy(networkAddr+1, (unsigned char *)sid, 2);
+ memcpy(networkAddr+3, (unsigned char *)ac_mac, 6);
+}
+
+static void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr,
+ unsigned int *ipAddr)
+{
+ memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+ networkAddr[0] = NAT25_IPV6;
+ memcpy(networkAddr+1, (unsigned char *)ipAddr, 16);
+}
+
+static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b)
+{
+ while (len > 0) {
+ if (*data == tag && *(data+1) == len8b && len >= len8b*8)
+ return data+2;
+
+ len -= (*(data+1))*8;
+ data += (*(data+1))*8;
+ }
+ return NULL;
+}
+
+static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac)
+{
+ struct icmp6hdr *icmphdr = (struct icmp6hdr *)data;
+ unsigned char *mac;
+
+ if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) {
+ if (len >= 8) {
+ mac = scan_tlv(&data[8], len-8, 1, 1);
+ if (mac) {
+ _DEBUG_INFO("Router Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+ replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+ memcpy(mac, replace_mac, 6);
+ return 1;
+ }
+ }
+ } else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) {
+ if (len >= 16) {
+ mac = scan_tlv(&data[16], len-16, 1, 1);
+ if (mac) {
+ _DEBUG_INFO("Router Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+ replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+ memcpy(mac, replace_mac, 6);
+ return 1;
+ }
+ }
+ } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
+ if (len >= 24) {
+ mac = scan_tlv(&data[24], len-24, 1, 1);
+ if (mac) {
+ _DEBUG_INFO("Neighbor Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+ replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+ memcpy(mac, replace_mac, 6);
+ return 1;
+ }
+ }
+ } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) {
+ if (len >= 24) {
+ mac = scan_tlv(&data[24], len-24, 2, 1);
+ if (mac) {
+ _DEBUG_INFO("Neighbor Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+ replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+ memcpy(mac, replace_mac, 6);
+ return 1;
+ }
+ }
+ } else if (icmphdr->icmp6_type == NDISC_REDIRECT) {
+ if (len >= 40) {
+ mac = scan_tlv(&data[40], len-40, 2, 1);
+ if (mac) {
+ _DEBUG_INFO("Redirect, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+ replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+ memcpy(mac, replace_mac, 6);
+ return 1;
+ }
+ }
+ }
+ return 0;
+}
+
+static inline int __nat25_network_hash(unsigned char *networkAddr)
+{
+ if (networkAddr[0] == NAT25_IPV4) {
+ unsigned long x;
+
+ x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ } else if (networkAddr[0] == NAT25_IPX) {
+ unsigned long x;
+
+ x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ } else if (networkAddr[0] == NAT25_APPLE) {
+ unsigned long x;
+
+ x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ } else if (networkAddr[0] == NAT25_PPPOE) {
+ unsigned long x;
+
+ x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ } else if (networkAddr[0] == NAT25_IPV6) {
+ unsigned long x;
+
+ x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^
+ networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^
+ networkAddr[16];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ } else {
+ unsigned long x = 0;
+ int i;
+
+ for (i = 0; i < MAX_NETWORK_ADDR_LEN; i++)
+ x ^= networkAddr[i];
+
+ return x & (NAT25_HASH_SIZE - 1);
+ }
+}
+
+static inline void __network_hash_link(struct adapter *priv,
+ struct nat25_network_db_entry *ent, int hash)
+{
+ /* Caller must spin_lock already! */
+ ent->next_hash = priv->nethash[hash];
+ if (ent->next_hash != NULL)
+ ent->next_hash->pprev_hash = &ent->next_hash;
+ priv->nethash[hash] = ent;
+ ent->pprev_hash = &priv->nethash[hash];
+}
+
+static inline void __network_hash_unlink(struct nat25_network_db_entry *ent)
+{
+ /* Caller must spin_lock already! */
+ *(ent->pprev_hash) = ent->next_hash;
+ if (ent->next_hash != NULL)
+ ent->next_hash->pprev_hash = ent->pprev_hash;
+ ent->next_hash = NULL;
+ ent->pprev_hash = NULL;
+}
+
+static int __nat25_db_network_lookup_and_replace(struct adapter *priv,
+ struct sk_buff *skb, unsigned char *networkAddr)
+{
+ struct nat25_network_db_entry *db;
+
+ spin_lock_bh(&priv->br_ext_lock);
+
+ db = priv->nethash[__nat25_network_hash(networkAddr)];
+ while (db != NULL) {
+ if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+ if (!__nat25_has_expired(priv, db)) {
+ /* replace the destination mac address */
+ memcpy(skb->data, db->macAddr, ETH_ALEN);
+ atomic_inc(&db->use_count);
+
+ DEBUG_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+ "%02x%02x%02x%02x%02x%02x\n",
+ db->macAddr[0],
+ db->macAddr[1],
+ db->macAddr[2],
+ db->macAddr[3],
+ db->macAddr[4],
+ db->macAddr[5],
+ db->networkAddr[0],
+ db->networkAddr[1],
+ db->networkAddr[2],
+ db->networkAddr[3],
+ db->networkAddr[4],
+ db->networkAddr[5],
+ db->networkAddr[6],
+ db->networkAddr[7],
+ db->networkAddr[8],
+ db->networkAddr[9],
+ db->networkAddr[10],
+ db->networkAddr[11],
+ db->networkAddr[12],
+ db->networkAddr[13],
+ db->networkAddr[14],
+ db->networkAddr[15],
+ db->networkAddr[16]);
+ }
+ spin_unlock_bh(&priv->br_ext_lock);
+ return 1;
+ }
+ db = db->next_hash;
+ }
+ spin_unlock_bh(&priv->br_ext_lock);
+ return 0;
+}
+
+static void __nat25_db_network_insert(struct adapter *priv,
+ unsigned char *macAddr, unsigned char *networkAddr)
+{
+ struct nat25_network_db_entry *db;
+ int hash;
+
+ spin_lock_bh(&priv->br_ext_lock);
+ hash = __nat25_network_hash(networkAddr);
+ db = priv->nethash[hash];
+ while (db != NULL) {
+ if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+ memcpy(db->macAddr, macAddr, ETH_ALEN);
+ db->ageing_timer = jiffies;
+ spin_unlock_bh(&priv->br_ext_lock);
+ return;
+ }
+ db = db->next_hash;
+ }
+ db = (struct nat25_network_db_entry *) rtw_malloc(sizeof(*db));
+ if (db == NULL) {
+ spin_unlock_bh(&priv->br_ext_lock);
+ return;
+ }
+ memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN);
+ memcpy(db->macAddr, macAddr, ETH_ALEN);
+ atomic_set(&db->use_count, 1);
+ db->ageing_timer = jiffies;
+
+ __network_hash_link(priv, db, hash);
+
+ spin_unlock_bh(&priv->br_ext_lock);
+}
+
+static void __nat25_db_print(struct adapter *priv)
+{
+}
+
+/*
+ * NAT2.5 interface
+ */
+
+void nat25_db_cleanup(struct adapter *priv)
+{
+ int i;
+
+ spin_lock_bh(&priv->br_ext_lock);
+
+ for (i = 0; i < NAT25_HASH_SIZE; i++) {
+ struct nat25_network_db_entry *f;
+ f = priv->nethash[i];
+ while (f != NULL) {
+ struct nat25_network_db_entry *g;
+
+ g = f->next_hash;
+ if (priv->scdb_entry == f) {
+ memset(priv->scdb_mac, 0, ETH_ALEN);
+ memset(priv->scdb_ip, 0, 4);
+ priv->scdb_entry = NULL;
+ }
+ __network_hash_unlink(f);
+ kfree(f);
+ f = g;
+ }
+ }
+ spin_unlock_bh(&priv->br_ext_lock);
+}
+
+void nat25_db_expire(struct adapter *priv)
+{
+ int i;
+
+ spin_lock_bh(&priv->br_ext_lock);
+
+ for (i = 0; i < NAT25_HASH_SIZE; i++) {
+ struct nat25_network_db_entry *f;
+ f = priv->nethash[i];
+
+ while (f != NULL) {
+ struct nat25_network_db_entry *g;
+ g = f->next_hash;
+
+ if (__nat25_has_expired(priv, f)) {
+ if (atomic_dec_and_test(&f->use_count)) {
+ if (priv->scdb_entry == f) {
+ memset(priv->scdb_mac, 0, ETH_ALEN);
+ memset(priv->scdb_ip, 0, 4);
+ priv->scdb_entry = NULL;
+ }
+ __network_hash_unlink(f);
+ kfree(f);
+ }
+ }
+ f = g;
+ }
+ }
+ spin_unlock_bh(&priv->br_ext_lock);
+}
+
+int nat25_db_handle(struct adapter *priv, struct sk_buff *skb, int method)
+{
+ unsigned short protocol;
+ unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+ unsigned int tmp;
+
+ if (skb == NULL)
+ return -1;
+
+ if ((method <= NAT25_MIN) || (method >= NAT25_MAX))
+ return -1;
+
+ protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
+
+ /*---------------------------------------------------*/
+ /* Handle IP frame */
+ /*---------------------------------------------------*/
+ if (protocol == ETH_P_IP) {
+ struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+ if (((unsigned char *)(iph) + (iph->ihl<<2)) >= (skb->data + ETH_HLEN + skb->len)) {
+ DEBUG_WARN("NAT25: malformed IP packet !\n");
+ return -1;
+ }
+
+ switch (method) {
+ case NAT25_CHECK:
+ return -1;
+ case NAT25_INSERT:
+ /* some multicast with source IP is all zero, maybe other case is illegal */
+ /* in class A, B, C, host address is all zero or all one is illegal */
+ if (iph->saddr == 0)
+ return 0;
+ tmp = be32_to_cpu(iph->saddr);
+ DEBUG_INFO("NAT25: Insert IP, SA =%08x, DA =%08x\n", tmp, iph->daddr);
+ __nat25_generate_ipv4_network_addr(networkAddr, &tmp);
+ /* record source IP address and , source mac address into db */
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+ __nat25_db_print(priv);
+ return 0;
+ case NAT25_LOOKUP:
+ DEBUG_INFO("NAT25: Lookup IP, SA =%08x, DA =%08x\n", iph->saddr, iph->daddr);
+ tmp = be32_to_cpu(iph->daddr);
+ __nat25_generate_ipv4_network_addr(networkAddr, &tmp);
+
+ if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
+ if (*((unsigned char *)&iph->daddr + 3) == 0xff) {
+ /* L2 is unicast but L3 is broadcast, make L2 bacome broadcast */
+ DEBUG_INFO("NAT25: Set DA as boardcast\n");
+ memset(skb->data, 0xff, ETH_ALEN);
+ } else {
+ /* forward unknow IP packet to upper TCP/IP */
+ DEBUG_INFO("NAT25: Replace DA with BR's MAC\n");
+ if ((*(u32 *)priv->br_mac) == 0 && (*(u16 *)(priv->br_mac+4)) == 0) {
+ printk("Re-init netdev_br_init() due to br_mac == 0!\n");
+ netdev_br_init(priv->pnetdev);
+ }
+ memcpy(skb->data, priv->br_mac, ETH_ALEN);
+ }
+ }
+ return 0;
+ default:
+ return -1;
+ }
+ } else if (protocol == ETH_P_ARP) {
+ /*---------------------------------------------------*/
+ /* Handle ARP frame */
+ /*---------------------------------------------------*/
+ struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+ unsigned char *arp_ptr = (unsigned char *)(arp + 1);
+ unsigned int *sender, *target;
+
+ if (arp->ar_pro != __constant_htons(ETH_P_IP)) {
+ DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", be16_to_cpu(arp->ar_pro));
+ return -1;
+ }
+
+ switch (method) {
+ case NAT25_CHECK:
+ return 0; /* skb_copy for all ARP frame */
+ case NAT25_INSERT:
+ DEBUG_INFO("NAT25: Insert ARP, MAC =%02x%02x%02x%02x%02x%02x\n", arp_ptr[0],
+ arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]);
+
+ /* change to ARP sender mac address to wlan STA address */
+ memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN);
+ arp_ptr += arp->ar_hln;
+ sender = (unsigned int *)arp_ptr;
+ __nat25_generate_ipv4_network_addr(networkAddr, sender);
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+ __nat25_db_print(priv);
+ return 0;
+ case NAT25_LOOKUP:
+ DEBUG_INFO("NAT25: Lookup ARP\n");
+
+ arp_ptr += arp->ar_hln;
+ sender = (unsigned int *)arp_ptr;
+ arp_ptr += (arp->ar_hln + arp->ar_pln);
+ target = (unsigned int *)arp_ptr;
+ __nat25_generate_ipv4_network_addr(networkAddr, target);
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+ /* change to ARP target mac address to Lookup result */
+ arp_ptr = (unsigned char *)(arp + 1);
+ arp_ptr += (arp->ar_hln + arp->ar_pln);
+ memcpy(arp_ptr, skb->data, ETH_ALEN);
+ return 0;
+ default:
+ return -1;
+ }
+ } else if ((protocol == ETH_P_IPX) ||
+ (protocol <= ETH_FRAME_LEN)) {
+ /*---------------------------------------------------*/
+ /* Handle IPX and Apple Talk frame */
+ /*---------------------------------------------------*/
+ unsigned char ipx_header[2] = {0xFF, 0xFF};
+ struct ipxhdr *ipx = NULL;
+ struct elapaarp *ea = NULL;
+ struct ddpehdr *ddp = NULL;
+ unsigned char *framePtr = skb->data + ETH_HLEN;
+
+ if (protocol == ETH_P_IPX) {
+ DEBUG_INFO("NAT25: Protocol = IPX (Ethernet II)\n");
+ ipx = (struct ipxhdr *)framePtr;
+ } else if (protocol <= ETH_FRAME_LEN) {
+ if (!memcmp(ipx_header, framePtr, 2)) {
+ DEBUG_INFO("NAT25: Protocol = IPX (Ethernet 802.3)\n");
+ ipx = (struct ipxhdr *)framePtr;
+ } else {
+ unsigned char ipx_8022_type = 0xE0;
+ unsigned char snap_8022_type = 0xAA;
+
+ if (*framePtr == snap_8022_type) {
+ unsigned char ipx_snap_id[5] = {0x0, 0x0, 0x0, 0x81, 0x37}; /* IPX SNAP ID */
+ unsigned char aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3}; /* Apple Talk AARP SNAP ID */
+ unsigned char ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B}; /* Apple Talk DDP SNAP ID */
+
+ framePtr += 3; /* eliminate the 802.2 header */
+
+ if (!memcmp(ipx_snap_id, framePtr, 5)) {
+ framePtr += 5; /* eliminate the SNAP header */
+
+ DEBUG_INFO("NAT25: Protocol = IPX (Ethernet SNAP)\n");
+ ipx = (struct ipxhdr *)framePtr;
+ } else if (!memcmp(aarp_snap_id, framePtr, 5)) {
+ framePtr += 5; /* eliminate the SNAP header */
+
+ ea = (struct elapaarp *)framePtr;
+ } else if (!memcmp(ddp_snap_id, framePtr, 5)) {
+ framePtr += 5; /* eliminate the SNAP header */
+
+ ddp = (struct ddpehdr *)framePtr;
+ } else {
+ DEBUG_WARN("NAT25: Protocol = Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0],
+ framePtr[1], framePtr[2], framePtr[3], framePtr[4]);
+ return -1;
+ }
+ } else if (*framePtr == ipx_8022_type) {
+ framePtr += 3; /* eliminate the 802.2 header */
+
+ if (!memcmp(ipx_header, framePtr, 2)) {
+ DEBUG_INFO("NAT25: Protocol = IPX (Ethernet 802.2)\n");
+ ipx = (struct ipxhdr *)framePtr;
+ } else {
+ return -1;
+ }
+ } else {
+ return -1;
+ }
+ }
+ } else {
+ return -1;
+ }
+
+ /* IPX */
+ if (ipx != NULL) {
+ switch (method) {
+ case NAT25_CHECK:
+ if (!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN))
+ DEBUG_INFO("NAT25: Check IPX skb_copy\n");
+ return 0;
+ case NAT25_INSERT:
+ DEBUG_INFO("NAT25: Insert IPX, Dest =%08x,%02x%02x%02x%02x%02x%02x,%04x Source =%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
+ ipx->ipx_dest.net,
+ ipx->ipx_dest.node[0],
+ ipx->ipx_dest.node[1],
+ ipx->ipx_dest.node[2],
+ ipx->ipx_dest.node[3],
+ ipx->ipx_dest.node[4],
+ ipx->ipx_dest.node[5],
+ ipx->ipx_dest.sock,
+ ipx->ipx_source.net,
+ ipx->ipx_source.node[0],
+ ipx->ipx_source.node[1],
+ ipx->ipx_source.node[2],
+ ipx->ipx_source.node[3],
+ ipx->ipx_source.node[4],
+ ipx->ipx_source.node[5],
+ ipx->ipx_source.sock);
+
+ if (!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) {
+ DEBUG_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
+
+ __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
+
+ /* change IPX source node addr to wlan STA address */
+ memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN);
+ } else {
+ __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
+ }
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+ __nat25_db_print(priv);
+ return 0;
+ case NAT25_LOOKUP:
+ if (!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN)) {
+ DEBUG_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
+
+ __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
+
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+ /* replace IPX destination node addr with Lookup destination MAC addr */
+ memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN);
+ } else {
+ __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
+
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+ }
+ return 0;
+ default:
+ return -1;
+ }
+ } else if (ea != NULL) {
+ /* Sanity check fields. */
+ if (ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN) {
+ DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
+ return -1;
+ }
+
+ switch (method) {
+ case NAT25_CHECK:
+ return 0;
+ case NAT25_INSERT:
+ /* change to AARP source mac address to wlan STA address */
+ memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN);
+
+ DEBUG_INFO("NAT25: Insert AARP, Source =%d,%d Destination =%d,%d\n",
+ ea->pa_src_net,
+ ea->pa_src_node,
+ ea->pa_dst_net,
+ ea->pa_dst_node);
+
+ __nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
+
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+ __nat25_db_print(priv);
+ return 0;
+ case NAT25_LOOKUP:
+ DEBUG_INFO("NAT25: Lookup AARP, Source =%d,%d Destination =%d,%d\n",
+ ea->pa_src_net,
+ ea->pa_src_node,
+ ea->pa_dst_net,
+ ea->pa_dst_node);
+
+ __nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
+
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+ /* change to AARP destination mac address to Lookup result */
+ memcpy(ea->hw_dst, skb->data, ETH_ALEN);
+ return 0;
+ default:
+ return -1;
+ }
+ } else if (ddp != NULL) {
+ switch (method) {
+ case NAT25_CHECK:
+ return -1;
+ case NAT25_INSERT:
+ DEBUG_INFO("NAT25: Insert DDP, Source =%d,%d Destination =%d,%d\n",
+ ddp->deh_snet,
+ ddp->deh_snode,
+ ddp->deh_dnet,
+ ddp->deh_dnode);
+
+ __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
+
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+ __nat25_db_print(priv);
+ return 0;
+ case NAT25_LOOKUP:
+ DEBUG_INFO("NAT25: Lookup DDP, Source =%d,%d Destination =%d,%d\n",
+ ddp->deh_snet,
+ ddp->deh_snode,
+ ddp->deh_dnet,
+ ddp->deh_dnode);
+ __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+ return 0;
+ default:
+ return -1;
+ }
+ }
+
+ return -1;
+ } else if ((protocol == ETH_P_PPP_DISC) ||
+ (protocol == ETH_P_PPP_SES)) {
+ /*---------------------------------------------------*/
+ /* Handle PPPoE frame */
+ /*---------------------------------------------------*/
+ struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+ unsigned short *pMagic;
+
+ switch (method) {
+ case NAT25_CHECK:
+ if (ph->sid == 0)
+ return 0;
+ return 1;
+ case NAT25_INSERT:
+ if (ph->sid == 0) { /* Discovery phase according to tag */
+ if (ph->code == PADI_CODE || ph->code == PADR_CODE) {
+ if (priv->ethBrExtInfo.addPPPoETag) {
+ struct pppoe_tag *tag, *pOldTag;
+ unsigned char tag_buf[40];
+ int old_tag_len = 0;
+
+ tag = (struct pppoe_tag *)tag_buf;
+ pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+ if (pOldTag) { /* if SID existed, copy old value and delete it */
+ old_tag_len = ntohs(pOldTag->tag_len);
+ if (old_tag_len+TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
+ DEBUG_ERR("SID tag length too long!\n");
+ return -1;
+ }
+
+ memcpy(tag->tag_data+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN,
+ pOldTag->tag_data, old_tag_len);
+
+ if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN+old_tag_len) < 0) {
+ DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
+ return -1;
+ }
+ ph->length = htons(ntohs(ph->length)-TAG_HDR_LEN-old_tag_len);
+ }
+
+ tag->tag_type = PTT_RELAY_SID;
+ tag->tag_len = htons(MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN+old_tag_len);
+
+ /* insert the magic_code+client mac in relay tag */
+ pMagic = (unsigned short *)tag->tag_data;
+ *pMagic = htons(MAGIC_CODE);
+ memcpy(tag->tag_data+MAGIC_CODE_LEN, skb->data+ETH_ALEN, ETH_ALEN);
+
+ /* Add relay tag */
+ if (__nat25_add_pppoe_tag(skb, tag) < 0)
+ return -1;
+
+ DEBUG_INFO("NAT25: Insert PPPoE, forward %s packet\n",
+ (ph->code == PADI_CODE ? "PADI" : "PADR"));
+ } else { /* not add relay tag */
+ if (priv->pppoe_connection_in_progress &&
+ memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN)) {
+ DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
+ return -2;
+ }
+
+ if (priv->pppoe_connection_in_progress == 0)
+ memcpy(priv->pppoe_addr, skb->data+ETH_ALEN, ETH_ALEN);
+
+ priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+ }
+ } else {
+ return -1;
+ }
+ } else { /* session phase */
+ DEBUG_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
+
+ __nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
+
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+ __nat25_db_print(priv);
+
+ if (!priv->ethBrExtInfo.addPPPoETag &&
+ priv->pppoe_connection_in_progress &&
+ !memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
+ priv->pppoe_connection_in_progress = 0;
+ }
+ return 0;
+ case NAT25_LOOKUP:
+ if (ph->code == PADO_CODE || ph->code == PADS_CODE) {
+ if (priv->ethBrExtInfo.addPPPoETag) {
+ struct pppoe_tag *tag;
+ unsigned char *ptr;
+ unsigned short tagType, tagLen;
+ int offset = 0;
+
+ ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+ if (ptr == NULL) {
+ DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
+ return -1;
+ }
+
+ tag = (struct pppoe_tag *)ptr;
+ tagType = (unsigned short)((ptr[0] << 8) + ptr[1]);
+ tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]);
+
+ if ((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN))) {
+ DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
+ return -1;
+ }
+
+ pMagic = (unsigned short *)tag->tag_data;
+ if (ntohs(*pMagic) != MAGIC_CODE) {
+ DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
+ (ph->code == PADO_CODE ? "PADO" : "PADS"));
+ return -1;
+ }
+
+ memcpy(skb->data, tag->tag_data+MAGIC_CODE_LEN, ETH_ALEN);
+
+ if (tagLen > MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN)
+ offset = TAG_HDR_LEN;
+
+ if (skb_pull_and_merge(skb, ptr+offset, TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset) < 0) {
+ DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
+ return -1;
+ }
+ ph->length = htons(ntohs(ph->length)-(TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset));
+ if (offset > 0)
+ tag->tag_len = htons(tagLen-MAGIC_CODE_LEN-RTL_RELAY_TAG_LEN);
+
+ DEBUG_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
+ (ph->code == PADO_CODE ? "PADO" : "PADS"), skb->dev->name);
+ } else { /* not add relay tag */
+ if (!priv->pppoe_connection_in_progress) {
+ DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
+ return -1;
+ }
+ memcpy(skb->data, priv->pppoe_addr, ETH_ALEN);
+ priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+ }
+ } else {
+ if (ph->sid != 0) {
+ DEBUG_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
+ __nat25_generate_pppoe_network_addr(networkAddr, skb->data+ETH_ALEN, &(ph->sid));
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+ __nat25_db_print(priv);
+ } else {
+ return -1;
+ }
+ }
+ return 0;
+ default:
+ return -1;
+ }
+ } else if (protocol == 0x888e) {
+ /*---------------------------------------------------*/
+ /* Handle EAP frame */
+ /*---------------------------------------------------*/
+ switch (method) {
+ case NAT25_CHECK:
+ return -1;
+ case NAT25_INSERT:
+ return 0;
+ case NAT25_LOOKUP:
+ return 0;
+ default:
+ return -1;
+ }
+ } else if ((protocol == 0xe2ae) || (protocol == 0xe2af)) {
+ /*---------------------------------------------------*/
+ /* Handle C-Media proprietary frame */
+ /*---------------------------------------------------*/
+ switch (method) {
+ case NAT25_CHECK:
+ return -1;
+ case NAT25_INSERT:
+ return 0;
+ case NAT25_LOOKUP:
+ return 0;
+ default:
+ return -1;
+ }
+ } else if (protocol == ETH_P_IPV6) {
+ /*------------------------------------------------*/
+ /* Handle IPV6 frame */
+ /*------------------------------------------------*/
+ struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+
+ if (sizeof(*iph) >= (skb->len - ETH_HLEN)) {
+ DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
+ return -1;
+ }
+
+ switch (method) {
+ case NAT25_CHECK:
+ if (skb->data[0] & 1)
+ return 0;
+ return -1;
+ case NAT25_INSERT:
+ DEBUG_INFO("NAT25: Insert IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
+ " DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
+ iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
+ iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
+ iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
+ iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
+
+ if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
+ __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
+ __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+ __nat25_db_print(priv);
+
+ if (iph->nexthdr == IPPROTO_ICMPV6 &&
+ skb->len > (ETH_HLEN + sizeof(*iph) + 4)) {
+ if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
+ skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
+ struct icmp6hdr *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
+ hdr->icmp6_cksum = 0;
+ hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
+ iph->payload_len,
+ IPPROTO_ICMPV6,
+ csum_partial((__u8 *)hdr, iph->payload_len, 0));
+ }
+ }
+ }
+ return 0;
+ case NAT25_LOOKUP:
+ DEBUG_INFO("NAT25: Lookup IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x, DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
+ iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
+ iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
+ iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
+ iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
+ __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
+ __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+ return 0;
+ default:
+ return -1;
+ }
+ }
+ return -1;
+}
+
+int nat25_handle_frame(struct adapter *priv, struct sk_buff *skb)
+{
+ if (!(skb->data[0] & 1)) {
+ int is_vlan_tag = 0, i, retval = 0;
+ unsigned short vlan_hdr = 0;
+ unsigned short protocol;
+
+ protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
+ if (protocol == ETH_P_8021Q) {
+ is_vlan_tag = 1;
+ vlan_hdr = *((unsigned short *)(skb->data+ETH_ALEN*2+2));
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+ETH_ALEN*2+2-i*2)) = *((unsigned short *)(skb->data+ETH_ALEN*2-2-i*2));
+ skb_pull(skb, 4);
+ }
+
+ if (!priv->ethBrExtInfo.nat25_disable) {
+ spin_lock_bh(&priv->br_ext_lock);
+ /*
+ * This function look up the destination network address from
+ * the NAT2.5 database. Return value = -1 means that the
+ * corresponding network protocol is NOT support.
+ */
+ if (!priv->ethBrExtInfo.nat25sc_disable &&
+ (be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
+ !memcmp(priv->scdb_ip, skb->data+ETH_HLEN+16, 4)) {
+ memcpy(skb->data, priv->scdb_mac, ETH_ALEN);
+
+ spin_unlock_bh(&priv->br_ext_lock);
+ } else {
+ spin_unlock_bh(&priv->br_ext_lock);
+
+ retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+ }
+ } else {
+ if (((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
+ !memcmp(priv->br_ip, skb->data+ETH_HLEN+16, 4)) ||
+ ((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_ARP) &&
+ !memcmp(priv->br_ip, skb->data+ETH_HLEN+24, 4))) {
+ /* for traffic to upper TCP/IP */
+ retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+ }
+ }
+
+ if (is_vlan_tag) {
+ skb_push(skb, 4);
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+ *((__be16 *)(skb->data+ETH_ALEN*2)) = __constant_htons(ETH_P_8021Q);
+ *((unsigned short *)(skb->data+ETH_ALEN*2+2)) = vlan_hdr;
+ }
+
+ if (retval == -1) {
+ /* DEBUG_ERR("NAT25: Lookup fail!\n"); */
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
+#define SERVER_PORT 67
+#define CLIENT_PORT 68
+#define DHCP_MAGIC 0x63825363
+#define BROADCAST_FLAG 0x8000
+
+struct dhcpMessage {
+ u_int8_t op;
+ u_int8_t htype;
+ u_int8_t hlen;
+ u_int8_t hops;
+ u_int32_t xid;
+ u_int16_t secs;
+ u_int16_t flags;
+ u_int32_t ciaddr;
+ u_int32_t yiaddr;
+ u_int32_t siaddr;
+ u_int32_t giaddr;
+ u_int8_t chaddr[16];
+ u_int8_t sname[64];
+ u_int8_t file[128];
+ u_int32_t cookie;
+ u_int8_t options[308]; /* 312 - cookie */
+};
+
+void dhcp_flag_bcast(struct adapter *priv, struct sk_buff *skb)
+{
+ if (skb == NULL)
+ return;
+
+ if (!priv->ethBrExtInfo.dhcp_bcst_disable) {
+ __be16 protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
+
+ if (protocol == __constant_htons(ETH_P_IP)) { /* IP */
+ struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+ if (iph->protocol == IPPROTO_UDP) { /* UDP */
+ struct udphdr *udph = (struct udphdr *)((size_t)iph + (iph->ihl << 2));
+
+ if ((udph->source == __constant_htons(CLIENT_PORT)) &&
+ (udph->dest == __constant_htons(SERVER_PORT))) { /* DHCP request */
+ struct dhcpMessage *dhcph =
+ (struct dhcpMessage *)((size_t)udph + sizeof(struct udphdr));
+ u32 cookie = be32_to_cpu((__be32)dhcph->cookie);
+
+ if (cookie == DHCP_MAGIC) { /* match magic word */
+ if (!(dhcph->flags & htons(BROADCAST_FLAG))) {
+ /* if not broadcast */
+ register int sum = 0;
+
+ DEBUG_INFO("DHCP: change flag of DHCP request to broadcast.\n");
+ /* or BROADCAST flag */
+ dhcph->flags |= htons(BROADCAST_FLAG);
+ /* recalculate checksum */
+ sum = ~(udph->check) & 0xffff;
+ sum += be16_to_cpu(dhcph->flags);
+ while (sum >> 16)
+ sum = (sum & 0xffff) + (sum >> 16);
+ udph->check = ~sum;
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+void *scdb_findEntry(struct adapter *priv, unsigned char *macAddr,
+ unsigned char *ipAddr)
+{
+ unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+ struct nat25_network_db_entry *db;
+ int hash;
+
+ __nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
+ hash = __nat25_network_hash(networkAddr);
+ db = priv->nethash[hash];
+ while (db != NULL) {
+ if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+ return (void *)db;
+ }
+
+ db = db->next_hash;
+ }
+
+ return NULL;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_cmd.c b/drivers/staging/r8188eu/core/rtw_cmd.c
new file mode 100644
index 000000000000..3b76634f2090
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_cmd.c
@@ -0,0 +1,2206 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_CMD_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <cmd_osdep.h>
+#include <mlme_osdep.h>
+#include <rtw_br_ext.h>
+#include <rtw_mlme_ext.h>
+
+/*
+Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+No irqsave is necessary.
+*/
+
+int _rtw_init_cmd_priv (struct cmd_priv *pcmdpriv)
+{
+ int res = _SUCCESS;
+
+ sema_init(&(pcmdpriv->cmd_queue_sema), 0);
+ /* sema_init(&(pcmdpriv->cmd_done_sema), 0); */
+ sema_init(&(pcmdpriv->terminate_cmdthread_sema), 0);
+
+ _rtw_init_queue(&(pcmdpriv->cmd_queue));
+
+ /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+ pcmdpriv->cmd_seq = 1;
+
+ pcmdpriv->cmd_allocated_buf = kzalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
+ GFP_KERNEL);
+
+ if (pcmdpriv->cmd_allocated_buf == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf + CMDBUFF_ALIGN_SZ - ((size_t)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
+
+ pcmdpriv->rsp_allocated_buf = kzalloc(MAX_RSPSZ + 4, GFP_KERNEL);
+
+ if (pcmdpriv->rsp_allocated_buf == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 - ((size_t)(pcmdpriv->rsp_allocated_buf) & 3);
+
+ pcmdpriv->cmd_issued_cnt = 0;
+ pcmdpriv->cmd_done_cnt = 0;
+ pcmdpriv->rsp_cnt = 0;
+exit:
+
+ return res;
+}
+
+static void c2h_wk_callback(struct work_struct *work);
+
+int _rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+ int res = _SUCCESS;
+
+ /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+ ATOMIC_SET(&pevtpriv->event_seq, 0);
+ pevtpriv->evt_done_cnt = 0;
+
+ _init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
+ pevtpriv->c2h_wk_alive = false;
+ pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1);
+
+ return res;
+}
+
+void rtw_free_evt_priv(struct evt_priv *pevtpriv)
+{
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+rtw_free_evt_priv\n"));
+
+ _cancel_workitem_sync(&pevtpriv->c2h_wk);
+ while (pevtpriv->c2h_wk_alive)
+ rtw_msleep_os(10);
+
+ while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+ void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
+ if (c2h != NULL && c2h != (void *)pevtpriv)
+ kfree(c2h);
+ }
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("-rtw_free_evt_priv\n"));
+
+}
+
+void _rtw_free_cmd_priv (struct cmd_priv *pcmdpriv)
+{
+
+ if (pcmdpriv) {
+ _rtw_spinlock_free(&(pcmdpriv->cmd_queue.lock));
+
+ if (pcmdpriv->cmd_allocated_buf)
+ kfree(pcmdpriv->cmd_allocated_buf);
+
+ if (pcmdpriv->rsp_allocated_buf)
+ kfree(pcmdpriv->rsp_allocated_buf);
+ }
+
+}
+
+/*
+Calling Context:
+
+rtw_enqueue_cmd can only be called between kernel thread,
+since only spin_lock is used.
+
+ISR/Call-Back functions can't call this sub-function.
+
+*/
+
+int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
+{
+ unsigned long flags;
+
+ if (obj == NULL)
+ goto exit;
+
+ spin_lock_irqsave(&queue->lock, flags);
+
+ list_add_tail(&obj->list, &queue->queue);
+
+ spin_unlock_irqrestore(&queue->lock, flags);
+
+exit:
+
+ return _SUCCESS;
+}
+
+struct cmd_obj *_rtw_dequeue_cmd(struct __queue *queue)
+{
+ struct cmd_obj *obj;
+ unsigned long flags;
+
+ spin_lock_irqsave(&queue->lock, flags);
+ if (list_empty(&(queue->queue))) {
+ obj = NULL;
+ } else {
+ obj = container_of((&(queue->queue))->next, struct cmd_obj, list);
+ rtw_list_delete(&obj->list);
+ }
+
+ spin_unlock_irqrestore(&queue->lock, flags);
+
+ return obj;
+}
+
+u32 rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+ u32 res;
+
+ res = _rtw_init_cmd_priv (pcmdpriv);
+
+ return res;
+}
+
+u32 rtw_init_evt_priv (struct evt_priv *pevtpriv)
+{
+ int res;
+
+ res = _rtw_init_evt_priv(pevtpriv);
+
+ return res;
+}
+
+void rtw_free_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_cmd_priv\n"));
+ _rtw_free_cmd_priv(pcmdpriv);
+
+}
+
+static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+ u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
+
+ /* To decide allow or not */
+ if ((pcmdpriv->padapter->pwrctrlpriv.bHWPwrPindetect) &&
+ (!pcmdpriv->padapter->registrypriv.usbss_enable)) {
+ if (cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
+ if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
+ bAllow = true;
+ }
+ }
+
+ if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+ bAllow = true;
+
+ if ((!pcmdpriv->padapter->hw_init_completed && !bAllow) ||
+ !pcmdpriv->cmdthd_running) /* com_thread not running */
+ return _FAIL;
+ return _SUCCESS;
+}
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+ int res = _FAIL;
+ struct adapter *padapter = pcmdpriv->padapter;
+
+ if (cmd_obj == NULL)
+ goto exit;
+
+ cmd_obj->padapter = padapter;
+
+ res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+ if (_FAIL == res) {
+ rtw_free_cmd_obj(cmd_obj);
+ goto exit;
+ }
+
+ res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+ if (res == _SUCCESS)
+ up(&pcmdpriv->cmd_queue_sema);
+
+exit:
+
+ return res;
+}
+
+struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+ struct cmd_obj *cmd_obj;
+
+ cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
+
+ return cmd_obj;
+}
+
+void rtw_cmd_clr_isr(struct cmd_priv *pcmdpriv)
+{
+
+ pcmdpriv->cmd_done_cnt++;
+ /* up(&(pcmdpriv->cmd_done_sema)); */
+
+}
+
+void rtw_free_cmd_obj(struct cmd_obj *pcmd)
+{
+
+ if ((pcmd->cmdcode != _JoinBss_CMD_) && (pcmd->cmdcode != _CreateBss_CMD_)) {
+ /* free parmbuf in cmd_obj */
+ kfree(pcmd->parmbuf);
+ }
+
+ if (pcmd->rsp != NULL) {
+ if (pcmd->rspsz != 0) {
+ /* free rsp in cmd_obj */
+ kfree(pcmd->rsp);
+ }
+ }
+
+ /* free cmd_obj */
+ kfree(pcmd);
+
+}
+
+int rtw_cmd_thread(void *context)
+{
+ u8 ret;
+ struct cmd_obj *pcmd;
+ u8 *pcmdbuf;
+ u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
+ void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
+ struct adapter *padapter = (struct adapter *)context;
+ struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+
+ thread_enter("RTW_CMD_THREAD");
+
+ pcmdbuf = pcmdpriv->cmd_buf;
+
+ pcmdpriv->cmdthd_running = true;
+ up(&pcmdpriv->terminate_cmdthread_sema);
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
+
+ while (1) {
+ if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL)
+ break;
+
+ if (padapter->bDriverStopped ||
+ padapter->bSurpriseRemoved) {
+ DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+ __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+ break;
+ }
+_next:
+ if (padapter->bDriverStopped ||
+ padapter->bSurpriseRemoved) {
+ DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+ __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+ break;
+ }
+
+ pcmd = rtw_dequeue_cmd(pcmdpriv);
+ if (!pcmd)
+ continue;
+
+ if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) {
+ pcmd->res = H2C_DROPPED;
+ goto post_process;
+ }
+
+ pcmdpriv->cmd_issued_cnt++;
+
+ pcmd->cmdsz = _RND4((pcmd->cmdsz));/* _RND4 */
+
+ memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+
+ if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
+ cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+
+ if (cmd_hdl) {
+ ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+ pcmd->res = ret;
+ }
+
+ pcmdpriv->cmd_seq++;
+ } else {
+ pcmd->res = H2C_PARAMETERS_ERROR;
+ }
+
+ cmd_hdl = NULL;
+
+post_process:
+
+ /* call callback function for post-processed */
+ if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
+ pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+ if (pcmd_callback == NULL) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
+ rtw_free_cmd_obj(pcmd);
+ } else {
+ /* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
+ pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode));
+ rtw_free_cmd_obj(pcmd);
+ }
+
+ flush_signals_thread();
+
+ goto _next;
+ }
+ pcmdpriv->cmdthd_running = false;
+
+ /* free all cmd_obj resources */
+ do {
+ pcmd = rtw_dequeue_cmd(pcmdpriv);
+ if (pcmd == NULL)
+ break;
+
+ /* DBG_88E("%s: leaving... drop cmdcode:%u\n", __func__, pcmd->cmdcode); */
+
+ rtw_free_cmd_obj(pcmd);
+ } while (1);
+
+ up(&pcmdpriv->terminate_cmdthread_sema);
+
+ thread_exit();
+}
+
+u8 rtw_setstandby_cmd(struct adapter *padapter, uint action)
+{
+ struct cmd_obj *ph2c;
+ struct usb_suspend_parm *psetusbsuspend;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 ret = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ psetusbsuspend = kzalloc(sizeof(struct usb_suspend_parm), GFP_ATOMIC);
+ if (psetusbsuspend == NULL) {
+ kfree(ph2c);
+ ret = _FAIL;
+ goto exit;
+ }
+
+ psetusbsuspend->action = action;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetusbsuspend, GEN_CMD_CODE(_SetUsbSuspend));
+
+ ret = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return ret;
+}
+
+/*
+rtw_sitesurvey_cmd(~)
+ ### NOTE:#### (!!!!)
+ MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+*/
+u8 rtw_sitesurvey_cmd(struct adapter *padapter, struct ndis_802_11_ssid *ssid, int ssid_num,
+ struct rtw_ieee80211_channel *ch, int ch_num)
+{
+ u8 res = _FAIL;
+ struct cmd_obj *ph2c;
+ struct sitesurvey_parm *psurveyPara;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+ rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+ }
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+ p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
+ }
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL)
+ return _FAIL;
+
+ psurveyPara = kzalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC);
+ if (psurveyPara == NULL) {
+ kfree(ph2c);
+ return _FAIL;
+ }
+
+ rtw_free_network_queue(padapter, false);
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__));
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+ /* psurveyPara->bsslimit = 48; */
+ psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+ /* prepare ssid list */
+ if (ssid) {
+ int i;
+ for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
+ if (ssid[i].SsidLength) {
+ memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
+ psurveyPara->ssid_num++;
+ if (0)
+ DBG_88E(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
+ psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
+ }
+ }
+ }
+
+ /* prepare channel list */
+ if (ch) {
+ int i;
+ for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
+ if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
+ memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
+ psurveyPara->ch_num++;
+ if (0)
+ DBG_88E(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
+ psurveyPara->ch[i].hw_value);
+ }
+ }
+ }
+
+ set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+ if (res == _SUCCESS) {
+ pmlmepriv->scan_start_time = jiffies;
+
+ _set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+
+ rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
+
+ pmlmepriv->scan_interval = SCAN_INTERVAL;/* 30*2 sec = 60sec */
+ } else {
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+ }
+
+ return res;
+}
+
+u8 rtw_setdatarate_cmd(struct adapter *padapter, u8 *rateset)
+{
+ struct cmd_obj *ph2c;
+ struct setdatarate_parm *pbsetdataratepara;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pbsetdataratepara = kzalloc(sizeof(struct setdatarate_parm), GFP_ATOMIC);
+ if (pbsetdataratepara == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
+ pbsetdataratepara->mac_id = 5;
+ memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_setbasicrate_cmd(struct adapter *padapter, u8 *rateset)
+{
+ struct cmd_obj *ph2c;
+ struct setbasicrate_parm *pssetbasicratepara;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ pssetbasicratepara = kzalloc(sizeof(struct setbasicrate_parm), GFP_ATOMIC);
+
+ if (pssetbasicratepara == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara, _SetBasicRate_CMD_);
+
+ memcpy(pssetbasicratepara->basicrates, rateset, NumRates);
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+/*
+unsigned char rtw_setphy_cmd(unsigned char *adapter)
+
+1. be called only after rtw_update_registrypriv_dev_network(~) or mp testing program
+2. for AdHoc/Ap mode or mp mode?
+
+*/
+u8 rtw_setphy_cmd(struct adapter *padapter, u8 modem, u8 ch)
+{
+ struct cmd_obj *ph2c;
+ struct setphy_parm *psetphypara;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ psetphypara = kzalloc(sizeof(struct setphy_parm), GFP_ATOMIC);
+
+ if (psetphypara == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetphypara, _SetPhy_CMD_);
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("CH =%d, modem =%d", ch, modem));
+
+ psetphypara->modem = modem;
+ psetphypara->rfchannel = ch;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_setbbreg_cmd(struct adapter *padapter, u8 offset, u8 val)
+{
+ struct cmd_obj *ph2c;
+ struct writeBB_parm *pwritebbparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ pwritebbparm = kzalloc(sizeof(struct writeBB_parm), GFP_ATOMIC);
+
+ if (pwritebbparm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pwritebbparm, GEN_CMD_CODE(_SetBBReg));
+
+ pwritebbparm->offset = offset;
+ pwritebbparm->value = val;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_getbbreg_cmd(struct adapter *padapter, u8 offset, u8 *pval)
+{
+ struct cmd_obj *ph2c;
+ struct readBB_parm *prdbbparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ prdbbparm = kzalloc(sizeof(struct readBB_parm), GFP_ATOMIC);
+
+ if (prdbbparm == NULL) {
+ kfree(ph2c);
+ return _FAIL;
+ }
+
+ INIT_LIST_HEAD(&ph2c->list);
+ ph2c->cmdcode = GEN_CMD_CODE(_GetBBReg);
+ ph2c->parmbuf = (unsigned char *)prdbbparm;
+ ph2c->cmdsz = sizeof(struct readBB_parm);
+ ph2c->rsp = pval;
+ ph2c->rspsz = sizeof(struct readBB_rsp);
+
+ prdbbparm->offset = offset;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_setrfreg_cmd(struct adapter *padapter, u8 offset, u32 val)
+{
+ struct cmd_obj *ph2c;
+ struct writeRF_parm *pwriterfparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ pwriterfparm = kzalloc(sizeof(struct writeRF_parm), GFP_ATOMIC);
+
+ if (pwriterfparm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
+
+ pwriterfparm->offset = offset;
+ pwriterfparm->value = val;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_getrfreg_cmd(struct adapter *padapter, u8 offset, u8 *pval)
+{
+ struct cmd_obj *ph2c;
+ struct readRF_parm *prdrfparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ prdrfparm = kzalloc(sizeof(struct readRF_parm), GFP_ATOMIC);
+ if (prdrfparm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ INIT_LIST_HEAD(&ph2c->list);
+ ph2c->cmdcode = GEN_CMD_CODE(_GetRFReg);
+ ph2c->parmbuf = (unsigned char *)prdrfparm;
+ ph2c->cmdsz = sizeof(struct readRF_parm);
+ ph2c->rsp = pval;
+ ph2c->rspsz = sizeof(struct readRF_rsp);
+
+ prdrfparm->offset = offset;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+
+
+ kfree(pcmd->parmbuf);
+ kfree(pcmd);
+
+ if (padapter->registrypriv.mp_mode == 1)
+ padapter->mppriv.workparam.bcompleted = true;
+
+}
+
+void rtw_readtssi_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+
+
+ kfree(pcmd->parmbuf);
+ kfree(pcmd);
+
+ if (padapter->registrypriv.mp_mode == 1)
+ padapter->mppriv.workparam.bcompleted = true;
+
+}
+
+u8 rtw_createbss_cmd(struct adapter *padapter)
+{
+ struct cmd_obj *pcmd;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
+ u8 res = _SUCCESS;
+
+ rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+ if (pmlmepriv->assoc_ssid.SsidLength == 0)
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+ else
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+
+ pcmd = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (pcmd == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ INIT_LIST_HEAD(&pcmd->list);
+ pcmd->cmdcode = _CreateBss_CMD_;
+ pcmd->parmbuf = (unsigned char *)pdev_network;
+ pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+ pcmd->rsp = NULL;
+ pcmd->rspsz = 0;
+ pdev_network->Length = pcmd->cmdsz;
+ res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+
+ return res;
+}
+
+u8 rtw_createbss_cmd_ex(struct adapter *padapter, unsigned char *pbss, unsigned int sz)
+{
+ struct cmd_obj *pcmd;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (pcmd == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ INIT_LIST_HEAD(&pcmd->list);
+ pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
+ pcmd->parmbuf = pbss;
+ pcmd->cmdsz = sz;
+ pcmd->rsp = NULL;
+ pcmd->rspsz = 0;
+
+ res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_joinbss_cmd(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+ u8 res = _SUCCESS;
+ uint t_len = 0;
+ struct wlan_bssid_ex *psecnetwork;
+ struct cmd_obj *pcmd;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+ enum ndis_802_11_network_infra ndis_network_mode = pnetwork->network.InfrastructureMode;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+ if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
+ } else {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid));
+ }
+
+ pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (pcmd == NULL) {
+ res = _FAIL;
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
+ goto exit;
+ }
+ /* for IEs is fix buf size */
+ t_len = sizeof(struct wlan_bssid_ex);
+
+ /* for hidden ap to set fw_state here */
+ if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
+ switch (ndis_network_mode) {
+ case Ndis802_11IBSS:
+ set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+ break;
+ case Ndis802_11Infrastructure:
+ set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+ break;
+ case Ndis802_11APMode:
+ case Ndis802_11AutoUnknown:
+ case Ndis802_11InfrastructureMax:
+ break;
+ }
+ }
+
+ psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+ if (psecnetwork == NULL) {
+ if (pcmd != NULL)
+ kfree(pcmd);
+
+ res = _FAIL;
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
+
+ goto exit;
+ }
+
+ memset(psecnetwork, 0, t_len);
+
+ memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
+
+ psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength;
+
+ if ((psecnetwork->IELength-12) < (256-1)) {
+ memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength-12);
+ } else {
+ memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256-1));
+ }
+
+ psecnetwork->IELength = 0;
+ /* Added by Albert 2009/02/18 */
+ /* If the the driver wants to use the bssid to create the connection. */
+ /* If not, we have to copy the connecting AP's MAC address to it so that */
+ /* the driver just has the bssid information for PMKIDList searching. */
+
+ if (!pmlmepriv->assoc_by_bssid)
+ memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN);
+
+ psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
+
+ pqospriv->qos_option = 0;
+
+ if (pregistrypriv->wmm_enable) {
+ u32 tmp_len;
+
+ tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
+
+ if (psecnetwork->IELength != tmp_len) {
+ psecnetwork->IELength = tmp_len;
+ pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
+ } else {
+ pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
+ }
+ }
+
+ phtpriv->ht_option = false;
+ if (pregistrypriv->ht_enable) {
+ /* Added by Albert 2010/06/23 */
+ /* For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
+ /* Especially for Realtek 8192u SoftAP. */
+ if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
+ (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
+ (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+ /* rtw_restructure_ht_ie */
+ rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0],
+ pnetwork->network.IELength, &psecnetwork->IELength);
+ }
+ }
+
+ pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+ if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
+ padapter->pwrctrlpriv.smart_ps = 0;
+ else
+ padapter->pwrctrlpriv.smart_ps = padapter->registrypriv.smart_ps;
+
+ DBG_88E("%s: smart_ps =%d\n", __func__, padapter->pwrctrlpriv.smart_ps);
+
+ pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
+
+ INIT_LIST_HEAD(&pcmd->list);
+ pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
+ pcmd->parmbuf = (unsigned char *)psecnetwork;
+ pcmd->rsp = NULL;
+ pcmd->rspsz = 0;
+
+ res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
+{
+ struct cmd_obj *cmdobj = NULL;
+ struct disconnect_parm *param = NULL;
+ struct cmd_priv *cmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
+
+ /* prepare cmd parameter */
+ param = kzalloc(sizeof(*param), GFP_ATOMIC);
+ if (param == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ param->deauth_timeout_ms = deauth_timeout_ms;
+
+ if (enqueue) {
+ /* need enqueue, prepare cmd_obj and enqueue */
+ cmdobj = kzalloc(sizeof(*cmdobj), GFP_ATOMIC);
+ if (cmdobj == NULL) {
+ res = _FAIL;
+ kfree(param);
+ goto exit;
+ }
+ init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+ res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+ } else {
+ /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+ if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
+ res = _FAIL;
+ kfree(param);
+ }
+
+exit:
+
+ return res;
+}
+
+u8 rtw_setopmode_cmd(struct adapter *padapter, enum ndis_802_11_network_infra networktype)
+{
+ struct cmd_obj *ph2c;
+ struct setopmode_parm *psetop;
+
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = false;
+ goto exit;
+ }
+ psetop = kzalloc(sizeof(struct setopmode_parm), GFP_KERNEL);
+
+ if (psetop == NULL) {
+ kfree(ph2c);
+ res = false;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+ psetop->mode = (u8)networktype;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
+{
+ struct cmd_obj *ph2c;
+ struct set_stakey_parm *psetstakey_para;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct set_stakey_rsp *psetstakey_rsp = NULL;
+
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct sta_info *sta = (struct sta_info *)psta;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_KERNEL);
+ if (psetstakey_para == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_KERNEL);
+ if (psetstakey_rsp == NULL) {
+ kfree(ph2c);
+ kfree(psetstakey_para);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+ ph2c->rsp = (u8 *)psetstakey_rsp;
+ ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+ memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+ psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
+ else
+ GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
+
+ if (unicast_key)
+ memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+ else
+ memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+
+ /* jeff: set this because at least sw key is ready */
+ padapter->securitypriv.busetkipkey = true;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
+{
+ struct cmd_obj *ph2c;
+ struct set_stakey_parm *psetstakey_para;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct set_stakey_rsp *psetstakey_rsp = NULL;
+ struct sta_info *sta = (struct sta_info *)psta;
+ u8 res = _SUCCESS;
+
+ if (!enqueue) {
+ clear_cam_entry(padapter, entry);
+ } else {
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetstakey_para = kzalloc(sizeof(struct set_stakey_parm),
+ GFP_ATOMIC);
+ if (psetstakey_para == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp),
+ GFP_ATOMIC);
+ if (psetstakey_rsp == NULL) {
+ kfree(ph2c);
+ kfree(psetstakey_para);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+ ph2c->rsp = (u8 *)psetstakey_rsp;
+ ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+ memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+ psetstakey_para->algorithm = _NO_PRIVACY_;
+
+ psetstakey_para->id = entry;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+ }
+exit:
+
+ return res;
+}
+
+u8 rtw_setrttbl_cmd(struct adapter *padapter, struct setratable_parm *prate_table)
+{
+ struct cmd_obj *ph2c;
+ struct setratable_parm *psetrttblparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ psetrttblparm = kzalloc(sizeof(struct setratable_parm), GFP_KERNEL);
+
+ if (psetrttblparm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
+
+ memcpy(psetrttblparm, prate_table, sizeof(struct setratable_parm));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_getrttbl_cmd(struct adapter *padapter, struct getratable_rsp *pval)
+{
+ struct cmd_obj *ph2c;
+ struct getratable_parm *pgetrttblparm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ pgetrttblparm = kzalloc(sizeof(struct getratable_parm), GFP_KERNEL);
+
+ if (pgetrttblparm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+/* init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable)); */
+
+ INIT_LIST_HEAD(&ph2c->list);
+ ph2c->cmdcode = GEN_CMD_CODE(_GetRaTable);
+ ph2c->parmbuf = (unsigned char *)pgetrttblparm;
+ ph2c->cmdsz = sizeof(struct getratable_parm);
+ ph2c->rsp = (u8 *)pval;
+ ph2c->rspsz = sizeof(struct getratable_rsp);
+
+ pgetrttblparm->rsvd = 0x0;
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_setassocsta_cmd(struct adapter *padapter, u8 *mac_addr)
+{
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct cmd_obj *ph2c;
+ struct set_assocsta_parm *psetassocsta_para;
+ struct set_stakey_rsp *psetassocsta_rsp = NULL;
+
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetassocsta_para = kzalloc(sizeof(struct set_assocsta_parm), GFP_ATOMIC);
+ if (psetassocsta_para == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ psetassocsta_rsp = kzalloc(sizeof(struct set_assocsta_rsp), GFP_ATOMIC);
+ if (psetassocsta_rsp == NULL) {
+ kfree(ph2c);
+ kfree(psetassocsta_para);
+ return _FAIL;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
+ ph2c->rsp = (u8 *)psetassocsta_rsp;
+ ph2c->rspsz = sizeof(struct set_assocsta_rsp);
+
+ memcpy(psetassocsta_para->addr, mac_addr, ETH_ALEN);
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+ }
+
+u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
+{
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct cmd_obj *ph2c;
+ struct addBaReq_parm *paddbareq_parm;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ paddbareq_parm = kzalloc(sizeof(struct addBaReq_parm), GFP_KERNEL);
+ if (paddbareq_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ paddbareq_parm->tid = tid;
+ memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
+
+ /* DBG_88E("rtw_addbareq_cmd, tid =%d\n", tid); */
+
+ /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
+ pdrvextra_cmd_parm->type_size = 0;
+ pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+u8 rtw_set_ch_cmd(struct adapter *padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue)
+{
+ struct cmd_obj *pcmdobj;
+ struct set_ch_parm *set_ch_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ DBG_88E(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+ FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
+
+ /* check input parameter */
+
+ /* prepare cmd parameter */
+ set_ch_parm = kzalloc(sizeof(*set_ch_parm), GFP_ATOMIC);
+ if (set_ch_parm == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ set_ch_parm->ch = ch;
+ set_ch_parm->bw = bw;
+ set_ch_parm->ch_offset = ch_offset;
+
+ if (enqueue) {
+ /* need enqueue, prepare cmd_obj and enqueue */
+ pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (pcmdobj == NULL) {
+ kfree(set_ch_parm);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, GEN_CMD_CODE(_SetChannel));
+ res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+ } else {
+ /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+ if (H2C_SUCCESS != set_ch_hdl(padapter, (u8 *)set_ch_parm))
+ res = _FAIL;
+
+ kfree(set_ch_parm);
+ }
+
+ /* do something based on res... */
+
+exit:
+
+ DBG_88E(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res);
+
+ return res;
+}
+
+u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue)
+{
+ struct cmd_obj *pcmdobj;
+ struct SetChannelPlan_param *setChannelPlan_param;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
+
+ /* check input parameter */
+ if (!rtw_is_channel_plan_valid(chplan)) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ /* prepare cmd parameter */
+ setChannelPlan_param = kzalloc(sizeof(struct SetChannelPlan_param),
+ GFP_KERNEL);
+ if (setChannelPlan_param == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ setChannelPlan_param->channel_plan = chplan;
+
+ if (enqueue) {
+ /* need enqueue, prepare cmd_obj and enqueue */
+ pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (pcmdobj == NULL) {
+ kfree(setChannelPlan_param);
+ res = _FAIL;
+ goto exit;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
+ res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+ } else {
+ /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+ if (H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param))
+ res = _FAIL;
+
+ kfree(setChannelPlan_param);
+ }
+
+ /* do something based on res... */
+ if (res == _SUCCESS)
+ padapter->mlmepriv.ChannelPlan = chplan;
+
+exit:
+
+ return res;
+}
+
+u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed)
+{
+ struct cmd_obj *pcmdobj;
+ struct LedBlink_param *ledBlink_param;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
+
+ pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (pcmdobj == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ ledBlink_param = kzalloc(sizeof(struct LedBlink_param), GFP_ATOMIC);
+ if (ledBlink_param == NULL) {
+ kfree(pcmdobj);
+ res = _FAIL;
+ goto exit;
+ }
+
+ ledBlink_param->pLed = pLed;
+
+ init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
+ res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no)
+{
+ struct cmd_obj *pcmdobj;
+ struct SetChannelSwitch_param *setChannelSwitch_param;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_csa_cmd\n"));
+
+ pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (pcmdobj == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ setChannelSwitch_param = kzalloc(sizeof(struct SetChannelSwitch_param),
+ GFP_ATOMIC);
+ if (setChannelSwitch_param == NULL) {
+ kfree(pcmdobj);
+ res = _FAIL;
+ goto exit;
+ }
+
+ setChannelSwitch_param->new_ch_no = new_ch_no;
+
+ init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelSwitch_param, GEN_CMD_CODE(_SetChannelSwitch));
+ res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+exit:
+
+ return res;
+}
+
+u8 rtw_tdls_cmd(struct adapter *padapter, u8 *addr, u8 option)
+{
+ return _SUCCESS;
+}
+
+static void traffic_status_watchdog(struct adapter *padapter)
+{
+ u8 bEnterPS;
+ u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
+ u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ /* */
+ /* Determine if our traffic is busy now */
+ /* */
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 100 ||
+ pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 100) {
+ bBusyTraffic = true;
+
+ if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+ bRxBusyTraffic = true;
+ else
+ bTxBusyTraffic = true;
+ }
+
+ /* Higher Tx/Rx data. */
+ if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+ pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+ bHigherBusyTraffic = true;
+
+ if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+ bHigherBusyRxTraffic = true;
+ else
+ bHigherBusyTxTraffic = true;
+ }
+
+ /* check traffic for powersaving. */
+ if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+ (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
+ bEnterPS = false;
+ else
+ bEnterPS = true;
+
+ /* LeisurePS only work in infra mode. */
+ if (bEnterPS)
+ LPS_Enter(padapter);
+ else
+ LPS_Leave(padapter);
+ } else {
+ LPS_Leave(padapter);
+ }
+
+ pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
+ pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
+ pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+ pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+ pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
+ pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
+ pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+ pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+ pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+}
+
+static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
+{
+ struct mlme_priv *pmlmepriv;
+
+ padapter = (struct adapter *)pbuf;
+ pmlmepriv = &(padapter->mlmepriv);
+
+#ifdef CONFIG_88EU_AP_MODE
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+ expire_timeout_chk(padapter);
+#endif
+
+ rtw_hal_sreset_xmit_status_check(padapter);
+
+ linked_status_chk(padapter);
+ traffic_status_watchdog(padapter);
+
+ rtw_hal_dm_watchdog(padapter);
+}
+
+static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ u8 mstatus;
+
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+ return;
+
+ switch (lps_ctrl_type) {
+ case LPS_CTRL_SCAN:
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+ /* connect */
+ LPS_Leave(padapter);
+ }
+ break;
+ case LPS_CTRL_JOINBSS:
+ LPS_Leave(padapter);
+ break;
+ case LPS_CTRL_CONNECT:
+ mstatus = 1;/* connect */
+ /* Reset LPS Setting */
+ padapter->pwrctrlpriv.LpsIdleCount = 0;
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+ break;
+ case LPS_CTRL_DISCONNECT:
+ mstatus = 0;/* disconnect */
+ LPS_Leave(padapter);
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+ break;
+ case LPS_CTRL_SPECIAL_PACKET:
+ /* DBG_88E("LPS_CTRL_SPECIAL_PACKET\n"); */
+ pwrpriv->DelayLPSLastTimeStamp = jiffies;
+ LPS_Leave(padapter);
+ break;
+ case LPS_CTRL_LEAVE:
+ LPS_Leave(padapter);
+ break;
+ default:
+ break;
+ }
+
+}
+
+u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ /* struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv; */
+ u8 res = _SUCCESS;
+
+ /* if (!pwrctrlpriv->bLeisurePs) */
+ /* return res; */
+
+ if (enqueue) {
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
+ GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
+ pdrvextra_cmd_parm->type_size = lps_ctrl_type;
+ pdrvextra_cmd_parm->pbuf = NULL;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+ } else {
+ lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
+ }
+
+exit:
+
+ return res;
+}
+
+static void rpt_timer_setting_wk_hdl(struct adapter *padapter, u16 min_time)
+{
+ rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&min_time));
+}
+
+u8 rtw_rpt_timer_cfg_cmd(struct adapter *padapter, u16 min_time)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
+ GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
+ pdrvextra_cmd_parm->type_size = min_time;
+ pdrvextra_cmd_parm->pbuf = NULL;
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+ return res;
+}
+
+static void antenna_select_wk_hdl(struct adapter *padapter, u8 antenna)
+{
+ rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_SELECT, (u8 *)(&antenna));
+}
+
+u8 rtw_antenna_select_cmd(struct adapter *padapter, u8 antenna, u8 enqueue)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 support_ant_div;
+ u8 res = _SUCCESS;
+
+ rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &support_ant_div);
+ if (!support_ant_div)
+ return res;
+
+ if (enqueue) {
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
+ GFP_KERNEL);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
+ pdrvextra_cmd_parm->type_size = antenna;
+ pdrvextra_cmd_parm->pbuf = NULL;
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+ } else {
+ antenna_select_wk_hdl(padapter, antenna);
+ }
+exit:
+
+ return res;
+}
+
+static void power_saving_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
+{
+ rtw_ps_processor(padapter);
+}
+
+#ifdef CONFIG_88EU_P2P
+u8 p2p_protocol_wk_cmd(struct adapter *padapter, int intCmdType)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return res;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
+ pdrvextra_cmd_parm->type_size = intCmdType; /* As the command tppe. */
+ pdrvextra_cmd_parm->pbuf = NULL; /* Must be NULL here */
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+#endif /* CONFIG_88EU_P2P */
+
+u8 rtw_ps_cmd(struct adapter *padapter)
+{
+ struct cmd_obj *ppscmd;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ u8 res = _SUCCESS;
+
+ ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ppscmd == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ppscmd);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
+ pdrvextra_cmd_parm->pbuf = NULL;
+ init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
+
+exit:
+
+ return res;
+}
+
+#ifdef CONFIG_88EU_AP_MODE
+
+static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
+{
+ int cnt = 0;
+ struct sta_info *psta_bmc;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ psta_bmc = rtw_get_bcmc_stainfo(padapter);
+ if (!psta_bmc)
+ return;
+
+ if (psta_bmc->sleepq_len == 0) {
+ u8 val = 0;
+
+ /* while ((rtw_read32(padapter, 0x414)&0x00ffff00)!= 0) */
+ /* while ((rtw_read32(padapter, 0x414)&0x0000ff00)!= 0) */
+
+ rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+
+ while (!val) {
+ rtw_msleep_os(100);
+
+ cnt++;
+
+ if (cnt > 10)
+ break;
+
+ rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+ }
+
+ if (cnt <= 10) {
+ pstapriv->tim_bitmap &= ~BIT(0);
+ pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+ } else { /* re check again */
+ rtw_chk_hi_queue_cmd(padapter);
+ }
+ }
+}
+
+u8 rtw_chk_hi_queue_cmd(struct adapter *padapter)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
+ pdrvextra_cmd_parm->type_size = 0;
+ pdrvextra_cmd_parm->pbuf = NULL;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+ return res;
+}
+#endif
+
+u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+ pdrvextra_cmd_parm->type_size = c2h_evt ? 16 : 0;
+ pdrvextra_cmd_parm->pbuf = c2h_evt;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+static s32 c2h_evt_hdl(struct adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter)
+{
+ s32 ret = _FAIL;
+ u8 buf[16];
+
+ if (!c2h_evt) {
+ /* No c2h event in cmd_obj, read c2h event before handling*/
+ if (c2h_evt_read(adapter, buf) == _SUCCESS) {
+ c2h_evt = (struct c2h_evt_hdr *)buf;
+
+ if (filter && filter(c2h_evt->id) == false)
+ goto exit;
+
+ ret = rtw_hal_c2h_handler(adapter, c2h_evt);
+ }
+ } else {
+ if (filter && filter(c2h_evt->id) == false)
+ goto exit;
+
+ ret = rtw_hal_c2h_handler(adapter, c2h_evt);
+ }
+exit:
+ return ret;
+}
+
+static void c2h_wk_callback(struct work_struct *work)
+{
+ struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+ struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv);
+ struct c2h_evt_hdr *c2h_evt;
+ c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
+
+ evtpriv->c2h_wk_alive = true;
+
+ while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+ if ((c2h_evt = (struct c2h_evt_hdr *)rtw_cbuf_pop(evtpriv->c2h_queue)) != NULL) {
+ /* This C2H event is read, clear it */
+ c2h_evt_clear(adapter);
+ } else if ((c2h_evt = (struct c2h_evt_hdr *)rtw_malloc(16)) != NULL) {
+ /* This C2H event is not read, read & clear now */
+ if (c2h_evt_read(adapter, (u8 *)c2h_evt) != _SUCCESS)
+ continue;
+ }
+
+ /* Special pointer to trigger c2h_evt_clear only */
+ if ((void *)c2h_evt == (void *)evtpriv)
+ continue;
+
+ if (!c2h_evt_exist(c2h_evt)) {
+ kfree(c2h_evt);
+ continue;
+ }
+
+ if (ccx_id_filter(c2h_evt->id) == true) {
+ /* Handle CCX report here */
+ rtw_hal_c2h_handler(adapter, c2h_evt);
+ kfree(c2h_evt);
+ } else {
+#ifdef CONFIG_88EU_P2P
+ /* Enqueue into cmd_thread for others */
+ rtw_c2h_wk_cmd(adapter, (u8 *)c2h_evt);
+#endif
+ }
+ }
+
+ evtpriv->c2h_wk_alive = false;
+}
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ struct drvextra_cmd_parm *pdrvextra_cmd;
+
+ if (!pbuf)
+ return H2C_PARAMETERS_ERROR;
+
+ pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
+
+ switch (pdrvextra_cmd->ec_id) {
+ case DYNAMIC_CHK_WK_CID:
+ dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
+ break;
+ case POWER_SAVING_CTRL_WK_CID:
+ power_saving_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
+ break;
+ case LPS_CTRL_WK_CID:
+ lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
+ break;
+ case RTP_TIMER_CFG_WK_CID:
+ rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type_size);
+ break;
+ case ANT_SELECT_WK_CID:
+ antenna_select_wk_hdl(padapter, pdrvextra_cmd->type_size);
+ break;
+#ifdef CONFIG_88EU_P2P
+ case P2P_PS_WK_CID:
+ p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type_size);
+ break;
+ case P2P_PROTO_WK_CID:
+ /* Commented by Albert 2011/07/01 */
+ /* I used the type_size as the type command */
+ p2p_protocol_wk_hdl(padapter, pdrvextra_cmd->type_size);
+ break;
+#endif
+#ifdef CONFIG_88EU_AP_MODE
+ case CHECK_HIQ_WK_CID:
+ rtw_chk_hi_queue_hdl(padapter);
+ break;
+#endif /* CONFIG_88EU_AP_MODE */
+ case C2H_WK_CID:
+ c2h_evt_hdl(padapter, (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
+ break;
+ default:
+ break;
+ }
+
+ if (pdrvextra_cmd->pbuf && pdrvextra_cmd->type_size > 0)
+ kfree(pdrvextra_cmd->pbuf);
+
+ return H2C_SUCCESS;
+}
+
+void rtw_survey_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if (pcmd->res == H2C_DROPPED) {
+ /* TODO: cancel timer and do timeout handler directly... */
+ /* need to make timeout handlerOS independent */
+ _set_timer(&pmlmepriv->scan_to_timer, 1);
+ } else if (pcmd->res != H2C_SUCCESS) {
+ _set_timer(&pmlmepriv->scan_to_timer, 1);
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
+ }
+
+ /* free cmd */
+ rtw_free_cmd_obj(pcmd);
+
+}
+void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if (pcmd->res != H2C_SUCCESS) {
+ spin_lock_bh(&pmlmepriv->lock);
+ set_fwstate(pmlmepriv, _FW_LINKED);
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
+ return;
+ } else /* clear bridge database */
+ nat25_db_cleanup(padapter);
+
+ /* free cmd */
+ rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_joinbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if (pcmd->res == H2C_DROPPED) {
+ /* TODO: cancel timer and do timeout handler directly... */
+ /* need to make timeout handlerOS independent */
+ _set_timer(&pmlmepriv->assoc_timer, 1);
+ } else if (pcmd->res != H2C_SUCCESS) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("********Error:rtw_select_and_join_from_scanned_queue Wait Sema Fail ************\n"));
+ _set_timer(&pmlmepriv->assoc_timer, 1);
+ }
+
+ rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ u8 timer_cancelled;
+ struct sta_info *psta = NULL;
+ struct wlan_network *pwlan = NULL;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
+ struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+
+ if ((pcmd->res != H2C_SUCCESS)) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback Fail ************\n\n."));
+ _set_timer(&pmlmepriv->assoc_timer, 1);
+ }
+
+ _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+ if (!psta) {
+ psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+ if (psta == NULL) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
+ goto createbss_cmd_fail ;
+ }
+ }
+
+ rtw_indicate_connect(padapter);
+ } else {
+
+ pwlan = _rtw_alloc_network(pmlmepriv);
+ spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+ if (pwlan == NULL) {
+ pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+ if (pwlan == NULL) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error: can't get pwlan in rtw_joinbss_event_callback\n"));
+ spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+ goto createbss_cmd_fail;
+ }
+ pwlan->last_scanned = jiffies;
+ } else {
+ list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+ }
+
+ pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
+ memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
+
+ memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
+
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+ spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+ /* we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
+ }
+
+createbss_cmd_fail:
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
+ struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
+
+ if (psta == NULL) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
+ goto exit;
+ }
+exit:
+ rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
+ struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
+ struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
+
+ if (psta == NULL) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
+ goto exit;
+ }
+
+ psta->aid = passocsta_rsp->cam_id;
+ psta->mac_id = passocsta_rsp->cam_id;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+ set_fwstate(pmlmepriv, _FW_LINKED);
+ spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+ rtw_free_cmd_obj(pcmd);
+
+}
diff --git a/drivers/staging/r8188eu/core/rtw_debug.c b/drivers/staging/r8188eu/core/rtw_debug.c
new file mode 100644
index 000000000000..47e5f7cf8453
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_debug.c
@@ -0,0 +1,943 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_DEBUG_C_
+
+#include <rtw_debug.h>
+#include <rtw_version.h>
+
+int proc_get_drv_version(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "%s\n", DRIVERVERSION);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_write_reg(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ *eof = 1;
+ return 0;
+}
+
+int proc_set_write_reg(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ char tmp[32];
+ u32 addr, val, len;
+
+ if (count < 3) {
+ DBG_88E("argument size is less than 3\n");
+ return -EFAULT;
+ }
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ int num = sscanf(tmp, "%x %x %x", &addr, &val, &len);
+
+ if (num != 3) {
+ DBG_88E("invalid write_reg parameter!\n");
+ return count;
+ }
+ switch (len) {
+ case 1:
+ rtw_write8(padapter, addr, (u8)val);
+ break;
+ case 2:
+ rtw_write16(padapter, addr, (u16)val);
+ break;
+ case 4:
+ rtw_write32(padapter, addr, val);
+ break;
+ default:
+ DBG_88E("error write length =%d", len);
+ break;
+ }
+ }
+ return count;
+}
+
+static u32 proc_get_read_addr = 0xeeeeeeee;
+static u32 proc_get_read_len = 0x4;
+
+int proc_get_read_reg(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+ int len = 0;
+
+ if (proc_get_read_addr == 0xeeeeeeee) {
+ *eof = 1;
+ return len;
+ }
+
+ switch (proc_get_read_len) {
+ case 1:
+ len += snprintf(page + len, count - len, "rtw_read8(0x%x)=0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr));
+ break;
+ case 2:
+ len += snprintf(page + len, count - len, "rtw_read16(0x%x)=0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr));
+ break;
+ case 4:
+ len += snprintf(page + len, count - len, "rtw_read32(0x%x)=0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr));
+ break;
+ default:
+ len += snprintf(page + len, count - len, "error read length=%d\n", proc_get_read_len);
+ break;
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_set_read_reg(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ char tmp[16];
+ u32 addr, len;
+
+ if (count < 2) {
+ DBG_88E("argument size is less than 2\n");
+ return -EFAULT;
+ }
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ int num = sscanf(tmp, "%x %x", &addr, &len);
+
+ if (num != 2) {
+ DBG_88E("invalid read_reg parameter!\n");
+ return count;
+ }
+
+ proc_get_read_addr = addr;
+
+ proc_get_read_len = len;
+ }
+
+ return count;
+}
+
+int proc_get_fwstate(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "fwstate=0x%x\n", get_fwstate(pmlmepriv));
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_sec_info(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "auth_alg=0x%x, enc_alg=0x%x, auth_type=0x%x, enc_type=0x%x\n",
+ psecuritypriv->dot11AuthAlgrthm, psecuritypriv->dot11PrivacyAlgrthm,
+ psecuritypriv->ndisauthtype, psecuritypriv->ndisencryptstatus);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_mlmext_state(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "pmlmeinfo->state=0x%x\n", pmlmeinfo->state);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_qos_option(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "qos_option=%d\n", pmlmepriv->qospriv.qos_option);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_ht_option(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ int len = 0;
+ len += snprintf(page + len, count - len, "ht_option=%d\n", pmlmepriv->htpriv.ht_option);
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rf_info(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "cur_ch=%d, cur_bw=%d, cur_ch_offet=%d\n",
+ pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+ *eof = 1;
+ return len;
+}
+
+int proc_get_ap_info(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct sta_info *psta;
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ int len = 0;
+
+ psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+ if (psta) {
+ int i;
+ struct recv_reorder_ctrl *preorder_ctrl;
+
+ len += snprintf(page + len, count - len, "SSID=%s\n", cur_network->network.Ssid.Ssid);
+ len += snprintf(page + len, count - len, "sta's macaddr:%pM\n", psta->hwaddr);
+ len += snprintf(page + len, count - len, "cur_channel=%d, cur_bwmode=%d, cur_ch_offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+ len += snprintf(page + len, count - len, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self);
+ len += snprintf(page + len, count - len, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+ len += snprintf(page + len, count - len, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+ len += snprintf(page + len, count - len, "bwmode=%d, ch_offset=%d, sgi=%d\n", psta->htpriv.bwmode, psta->htpriv.ch_offset, psta->htpriv.sgi);
+ len += snprintf(page + len, count - len, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+ len += snprintf(page + len, count - len, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+
+ for (i = 0; i < 16; i++) {
+ preorder_ctrl = &psta->recvreorder_ctrl[i];
+ if (preorder_ctrl->enable)
+ len += snprintf(page + len, count - len, "tid=%d, indicate_seq=%d\n", i, preorder_ctrl->indicate_seq);
+ }
+ } else {
+ len += snprintf(page + len, count - len, "can't get sta's macaddr, cur_network's macaddr: %pM\n", cur_network->network.MacAddress);
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_adapter_state(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "bSurpriseRemoved=%d, bDriverStopped=%d\n",
+ padapter->bSurpriseRemoved, padapter->bDriverStopped);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_trx_info(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "free_xmitbuf_cnt=%d, free_xmitframe_cnt=%d, free_ext_xmitbuf_cnt=%d, free_recvframe_cnt=%d\n",
+ pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt, pxmitpriv->free_xmit_extbuf_cnt, precvpriv->free_recvframe_cnt);
+ len += snprintf(page + len, count - len, "rx_urb_pending_cn=%d\n", precvpriv->rx_pending_cnt);
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_mac_reg_dump1(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+
+ for (i = 0x0; i < 0x300; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_mac_reg_dump2(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+ memset(page, 0, count);
+ for (i = 0x300; i < 0x600; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_mac_reg_dump3(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+
+ for (i = 0x600; i < 0x800; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_bb_reg_dump1(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= BB REG =======\n");
+ for (i = 0x800; i < 0xB00; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_bb_reg_dump2(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= BB REG =======\n");
+ for (i = 0xB00; i < 0xE00; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_bb_reg_dump3(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1;
+
+ len += snprintf(page + len, count - len, "\n======= BB REG =======\n");
+ for (i = 0xE00; i < 0x1000; i += 4) {
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", rtw_read32(padapter, i));
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rf_reg_dump1(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1, path;
+ u32 value;
+
+ len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+ path = 1;
+ len += snprintf(page + len, count - len, "\nRF_Path(%x)\n", path);
+ for (i = 0; i < 0xC0; i++) {
+ value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x ", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", value);
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rf_reg_dump2(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1, path;
+ u32 value;
+
+ len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+ path = 1;
+ len += snprintf(page + len, count - len, "\nRF_Path(%x)\n", path);
+ for (i = 0xC0; i < 0x100; i++) {
+ value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x ", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", value);
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rf_reg_dump3(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1, path;
+ u32 value;
+
+ len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+ path = 2;
+ len += snprintf(page + len, count - len, "\nRF_Path(%x)\n", path);
+ for (i = 0; i < 0xC0; i++) {
+ value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x ", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", value);
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rf_reg_dump4(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+ int i, j = 1, path;
+ u32 value;
+
+ len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+ path = 2;
+ len += snprintf(page + len, count - len, "\nRF_Path(%x)\n", path);
+ for (i = 0xC0; i < 0x100; i++) {
+ value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+ if (j%4 == 1)
+ len += snprintf(page + len, count - len, "0x%02x ", i);
+ len += snprintf(page + len, count - len, " 0x%08x ", value);
+ if ((j++)%4 == 0)
+ len += snprintf(page + len, count - len, "\n");
+ }
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rx_signal(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ int len = 0;
+
+ len = snprintf(page + len, count,
+ "rssi:%d\n"
+ "rxpwdb:%d\n"
+ "signal_strength:%u\n"
+ "signal_qual:%u\n"
+ "noise:%u\n",
+ padapter->recvpriv.rssi,
+ padapter->recvpriv.rxpwdb,
+ padapter->recvpriv.signal_strength,
+ padapter->recvpriv.signal_qual,
+ padapter->recvpriv.noise
+ );
+
+ *eof = 1;
+ return len;
+}
+
+int proc_set_rx_signal(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ char tmp[32];
+ u32 is_signal_dbg;
+ s32 signal_strength;
+
+ if (count < 1)
+ return -EFAULT;
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength);
+ is_signal_dbg = is_signal_dbg == 0 ? 0 : 1;
+ if (is_signal_dbg && num != 2)
+ return count;
+
+ signal_strength = signal_strength > 100 ? 100 : signal_strength;
+ signal_strength = signal_strength < 0 ? 0 : signal_strength;
+
+ padapter->recvpriv.is_signal_dbg = is_signal_dbg;
+ padapter->recvpriv.signal_strength_dbg = signal_strength;
+
+ if (is_signal_dbg)
+ DBG_88E("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength);
+ else
+ DBG_88E("set %s\n", "HW_SIGNAL_STRENGTH");
+ }
+ return count;
+}
+
+int proc_get_ht_enable(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ int len = 0;
+
+ if (pregpriv)
+ len += snprintf(page + len, count - len,
+ "%d\n",
+ pregpriv->ht_enable
+ );
+ *eof = 1;
+ return len;
+}
+
+int proc_set_ht_enable(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ char tmp[32];
+ s32 mode = 0;
+
+ if (count < 1)
+ return -EFAULT;
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ if (pregpriv) {
+ pregpriv->ht_enable = mode;
+ pr_info("ht_enable=%d\n", pregpriv->ht_enable);
+ }
+ }
+
+ return count;
+}
+
+int proc_get_cbw40_enable(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+
+ int len = 0;
+
+ if (pregpriv)
+ len += snprintf(page + len, count - len,
+ "%d\n",
+ pregpriv->cbw40_enable
+ );
+
+ *eof = 1;
+ return len;
+}
+
+int proc_set_cbw40_enable(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ char tmp[32];
+ s32 mode = 0;
+
+ if (count < 1)
+ return -EFAULT;
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ if (pregpriv) {
+ pregpriv->cbw40_enable = mode;
+ pr_info("cbw40_enable=%d\n", mode);
+ }
+ }
+ return count;
+}
+
+int proc_get_ampdu_enable(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+
+ int len = 0;
+
+ if (pregpriv)
+ len += snprintf(page + len, count - len,
+ "%d\n",
+ pregpriv->ampdu_enable
+ );
+
+ *eof = 1;
+ return len;
+}
+
+int proc_set_ampdu_enable(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ char tmp[32];
+ s32 mode = 0;
+
+ if (count < 1)
+ return -EFAULT;
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ if (pregpriv) {
+ pregpriv->ampdu_enable = mode;
+ pr_info("ampdu_enable=%d\n", mode);
+ }
+ }
+ return count;
+}
+
+int proc_get_two_path_rssi(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+ int len = 0;
+
+ if (padapter)
+ len += snprintf(page + len, count - len,
+ "%d %d\n",
+ padapter->recvpriv.RxRssi[0],
+ padapter->recvpriv.RxRssi[1]
+ );
+
+ *eof = 1;
+ return len;
+}
+
+int proc_get_rx_stbc(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+
+ int len = 0;
+
+ if (pregpriv)
+ len += snprintf(page + len, count - len,
+ "%d\n",
+ pregpriv->rx_stbc
+ );
+
+ *eof = 1;
+ return len;
+}
+
+int proc_set_rx_stbc(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ char tmp[32];
+ u32 mode = 0;
+
+ if (count < 1)
+ return -EFAULT;
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ if (pregpriv) {
+ pregpriv->rx_stbc = mode;
+ printk("rx_stbc=%d\n", mode);
+ }
+ }
+ return count;
+}
+
+int proc_get_rssi_disp(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ *eof = 1;
+ return 0;
+}
+
+int proc_set_rssi_disp(struct file *file, const char __user *buffer,
+ unsigned long count, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ char tmp[32];
+ u32 enable = 0;
+
+ if (count < 1) {
+ DBG_88E("argument size is less than 1\n");
+ return -EFAULT;
+ }
+
+ if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+ int num = sscanf(tmp, "%x", &enable);
+
+ if (num != 1) {
+ DBG_88E("invalid set_rssi_disp parameter!\n");
+ return count;
+ }
+
+ if (enable) {
+ DBG_88E("Turn On Rx RSSI Display Function\n");
+ padapter->bRxRSSIDisplay = enable ;
+ } else {
+ DBG_88E("Turn Off Rx RSSI Display Function\n");
+ padapter->bRxRSSIDisplay = 0;
+ }
+ }
+ return count;
+}
+
+#ifdef CONFIG_88EU_AP_MODE
+
+int proc_get_all_sta_info(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct sta_info *psta;
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ int i, j;
+ struct list_head *plist, *phead;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ int len = 0;
+
+ len += snprintf(page + len, count - len, "sta_dz_bitmap=0x%x, tim_bitmap=0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+
+ for (i = 0; i < NUM_STA; i++) {
+ phead = &(pstapriv->sta_hash[i]);
+ plist = phead->next;
+
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, hash_list);
+
+ plist = plist->next;
+
+ len += snprintf(page + len, count - len, "sta's macaddr: %pM\n", psta->hwaddr);
+ len += snprintf(page + len, count - len, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self);
+ len += snprintf(page + len, count - len, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+ len += snprintf(page + len, count - len, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+ len += snprintf(page + len, count - len, "bwmode=%d, ch_offset=%d, sgi=%d\n", psta->htpriv.bwmode, psta->htpriv.ch_offset, psta->htpriv.sgi);
+ len += snprintf(page + len, count - len, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+ len += snprintf(page + len, count - len, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+ len += snprintf(page + len, count - len, "sleepq_len=%d\n", psta->sleepq_len);
+ len += snprintf(page + len, count - len, "capability=0x%x\n", psta->capability);
+ len += snprintf(page + len, count - len, "flags=0x%x\n", psta->flags);
+ len += snprintf(page + len, count - len, "wpa_psk=0x%x\n", psta->wpa_psk);
+ len += snprintf(page + len, count - len, "wpa2_group_cipher=0x%x\n", psta->wpa2_group_cipher);
+ len += snprintf(page + len, count - len, "wpa2_pairwise_cipher=0x%x\n", psta->wpa2_pairwise_cipher);
+ len += snprintf(page + len, count - len, "qos_info=0x%x\n", psta->qos_info);
+ len += snprintf(page + len, count - len, "dot118021XPrivacy=0x%x\n", psta->dot118021XPrivacy);
+
+ for (j = 0; j < 16; j++) {
+ preorder_ctrl = &psta->recvreorder_ctrl[j];
+ if (preorder_ctrl->enable)
+ len += snprintf(page + len, count - len, "tid=%d, indicate_seq=%d\n", j, preorder_ctrl->indicate_seq);
+ }
+ }
+ }
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ *eof = 1;
+ return len;
+}
+#endif
+
+int proc_get_best_channel(char *page, char **start,
+ off_t offset, int count,
+ int *eof, void *data)
+{
+ struct net_device *dev = data;
+ struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ int len = 0;
+ u32 i, best_channel_24G = 1, best_channel_5G = 36, index_24G = 0, index_5G = 0;
+
+ for (i = 0; pmlmeext->channel_set[i].ChannelNum != 0; i++) {
+ if (pmlmeext->channel_set[i].ChannelNum == 1)
+ index_24G = i;
+ if (pmlmeext->channel_set[i].ChannelNum == 36)
+ index_5G = i;
+ }
+
+ for (i = 0; pmlmeext->channel_set[i].ChannelNum != 0; i++) {
+ /* 2.4G */
+ if (pmlmeext->channel_set[i].ChannelNum == 6) {
+ if (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_24G].rx_count) {
+ index_24G = i;
+ best_channel_24G = pmlmeext->channel_set[i].ChannelNum;
+ }
+ }
+
+ /* 5G */
+ if (pmlmeext->channel_set[i].ChannelNum >= 36 &&
+ pmlmeext->channel_set[i].ChannelNum < 140) {
+ /* Find primary channel */
+ if (((pmlmeext->channel_set[i].ChannelNum - 36) % 8 == 0) &&
+ (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count)) {
+ index_5G = i;
+ best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+ }
+ }
+
+ if (pmlmeext->channel_set[i].ChannelNum >= 149 &&
+ pmlmeext->channel_set[i].ChannelNum < 165) {
+ /* find primary channel */
+ if (((pmlmeext->channel_set[i].ChannelNum - 149) % 8 == 0) &&
+ (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count)) {
+ index_5G = i;
+ best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+ }
+ }
+ /* debug */
+ len += snprintf(page + len, count - len, "The rx cnt of channel %3d = %d\n",
+ pmlmeext->channel_set[i].ChannelNum, pmlmeext->channel_set[i].rx_count);
+ }
+
+ len += snprintf(page + len, count - len, "best_channel_5G = %d\n", best_channel_5G);
+ len += snprintf(page + len, count - len, "best_channel_24G = %d\n", best_channel_24G);
+
+ *eof = 1;
+ return len;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_efuse.c b/drivers/staging/r8188eu/core/rtw_efuse.c
new file mode 100644
index 000000000000..53b73f442699
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_efuse.c
@@ -0,0 +1,872 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_EFUSE_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_efuse.h>
+
+/*------------------------Define local variable------------------------------*/
+u8 fakeEfuseBank;
+u32 fakeEfuseUsedBytes;
+u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0};
+u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0};
+u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0};
+
+u32 BTEfuseUsedBytes;
+u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+u32 fakeBTEfuseUsedBytes;
+u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+/*------------------------Define local variable------------------------------*/
+
+/* */
+#define REG_EFUSE_CTRL 0x0030
+#define EFUSE_CTRL REG_EFUSE_CTRL /* E-Fuse Control. */
+/* */
+
+bool
+Efuse_Read1ByteFromFakeContent(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u8 *Value);
+bool
+Efuse_Read1ByteFromFakeContent(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u8 *Value)
+{
+ if (Offset >= EFUSE_MAX_HW_SIZE)
+ return false;
+ if (fakeEfuseBank == 0)
+ *Value = fakeEfuseContent[Offset];
+ else
+ *Value = fakeBTEfuseContent[fakeEfuseBank-1][Offset];
+ return true;
+}
+
+static bool
+Efuse_Write1ByteToFakeContent(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u8 Value)
+{
+ if (Offset >= EFUSE_MAX_HW_SIZE)
+ return false;
+ if (fakeEfuseBank == 0) {
+ fakeEfuseContent[Offset] = Value;
+ } else {
+ fakeBTEfuseContent[fakeEfuseBank-1][Offset] = Value;
+ }
+ return true;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: Efuse_PowerSwitch
+ *
+ * Overview: When we want to enable write operation, we should change to
+ * pwr on state. When we stop write, we should switch to 500k mode
+ * and disable LDO 2.5V.
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/17/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+Efuse_PowerSwitch(
+ struct adapter *pAdapter,
+ u8 write,
+ u8 PwrState)
+{
+ pAdapter->HalFunc.EfusePowerSwitch(pAdapter, write, PwrState);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: efuse_GetCurrentSize
+ *
+ * Overview: Get current efuse size!!!
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/16/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u16
+Efuse_GetCurrentSize(
+ struct adapter *pAdapter,
+ u8 efuseType,
+ bool pseudo)
+{
+ u16 ret = 0;
+
+ ret = pAdapter->HalFunc.EfuseGetCurrentSize(pAdapter, efuseType, pseudo);
+
+ return ret;
+}
+
+/* 11/16/2008 MH Add description. Get current efuse area enabled word!!. */
+u8
+Efuse_CalculateWordCnts(u8 word_en)
+{
+ u8 word_cnts = 0;
+ if (!(word_en & BIT(0)))
+ word_cnts++; /* 0 : write enable */
+ if (!(word_en & BIT(1)))
+ word_cnts++;
+ if (!(word_en & BIT(2)))
+ word_cnts++;
+ if (!(word_en & BIT(3)))
+ word_cnts++;
+ return word_cnts;
+}
+
+/* */
+/* Description: */
+/* Execute E-Fuse read byte operation. */
+/* Referred from SD1 Richard. */
+/* */
+/* Assumption: */
+/* 1. Boot from E-Fuse and successfully auto-load. */
+/* 2. PASSIVE_LEVEL (USB interface) */
+/* */
+/* Created by Roger, 2008.10.21. */
+/* */
+void
+ReadEFuseByte(
+ struct adapter *Adapter,
+ u16 _offset,
+ u8 *pbuf,
+ bool pseudo)
+{
+ u32 value32;
+ u8 readbyte;
+ u16 retry;
+
+ if (pseudo) {
+ Efuse_Read1ByteFromFakeContent(Adapter, _offset, pbuf);
+ return;
+ }
+
+ /* Write Address */
+ rtw_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));
+ readbyte = rtw_read8(Adapter, EFUSE_CTRL+2);
+ rtw_write8(Adapter, EFUSE_CTRL+2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+
+ /* Write bit 32 0 */
+ readbyte = rtw_read8(Adapter, EFUSE_CTRL+3);
+ rtw_write8(Adapter, EFUSE_CTRL+3, (readbyte & 0x7f));
+
+ /* Check bit 32 read-ready */
+ retry = 0;
+ value32 = rtw_read32(Adapter, EFUSE_CTRL);
+ while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) {
+ value32 = rtw_read32(Adapter, EFUSE_CTRL);
+ retry++;
+ }
+
+ /* 20100205 Joseph: Add delay suggested by SD1 Victor. */
+ /* This fix the problem that Efuse read error in high temperature condition. */
+ /* Designer says that there shall be some delay after ready bit is set, or the */
+ /* result will always stay on last data we read. */
+ rtw_udelay_os(50);
+ value32 = rtw_read32(Adapter, EFUSE_CTRL);
+
+ *pbuf = (u8)(value32 & 0xff);
+}
+
+/* */
+/* Description: */
+/* 1. Execute E-Fuse read byte operation according as map offset and */
+/* save to E-Fuse table. */
+/* 2. Referred from SD1 Richard. */
+/* */
+/* Assumption: */
+/* 1. Boot from E-Fuse and successfully auto-load. */
+/* 2. PASSIVE_LEVEL (USB interface) */
+/* */
+/* Created by Roger, 2008.10.21. */
+/* */
+/* 2008/12/12 MH 1. Reorganize code flow and reserve bytes. and add description. */
+/* 2. Add efuse utilization collect. */
+/* 2008/12/22 MH Read Efuse must check if we write section 1 data again!!! Sec1 */
+/* write addr must be after sec5. */
+/* */
+
+static void efuse_ReadEFuse(struct adapter *Adapter, u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf, bool pseudo)
+{
+ Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, pseudo);
+}
+
+void EFUSE_GetEfuseDefinition(struct adapter *pAdapter, u8 efuseType, u8 type, void *pOut, bool pseudo
+ )
+{
+ pAdapter->HalFunc.EFUSEGetEfuseDefinition(pAdapter, efuseType, type, pOut, pseudo);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: EFUSE_Read1Byte
+ *
+ * Overview: Copy from WMAC fot EFUSE read 1 byte.
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 09/23/2008 MHC Copy from WMAC.
+ *
+ *---------------------------------------------------------------------------*/
+u8 EFUSE_Read1Byte(struct adapter *Adapter, u16 Address)
+{
+ u8 data;
+ u8 Bytetemp = {0x00};
+ u8 temp = {0x00};
+ u32 k = 0;
+ u16 contentLen = 0;
+
+ EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI , TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&contentLen, false);
+
+ if (Address < contentLen) { /* E-fuse 512Byte */
+ /* Write E-fuse Register address bit0~7 */
+ temp = Address & 0xFF;
+ rtw_write8(Adapter, EFUSE_CTRL+1, temp);
+ Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+ /* Write E-fuse Register address bit8~9 */
+ temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
+ rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+
+ /* Write 0x30[31]= 0 */
+ Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+ temp = Bytetemp & 0x7F;
+ rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+
+ /* Wait Write-ready (0x30[31]= 1) */
+ Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+ while (!(Bytetemp & 0x80)) {
+ Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+ k++;
+ if (k == 1000) {
+ k = 0;
+ break;
+ }
+ }
+ data = rtw_read8(Adapter, EFUSE_CTRL);
+ return data;
+ } else {
+ return 0xFF;
+ }
+
+} /* EFUSE_Read1Byte */
+
+/* 11/16/2008 MH Read one byte from real Efuse. */
+u8 efuse_OneByteRead(struct adapter *pAdapter, u16 addr, u8 *data, bool pseudo)
+{
+ u8 tmpidx = 0;
+ u8 result;
+
+ if (pseudo) {
+ result = Efuse_Read1ByteFromFakeContent(pAdapter, addr, data);
+ return result;
+ }
+ /* -----------------e-fuse reg ctrl --------------------------------- */
+ /* address */
+ rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr & 0xff));
+ rtw_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) & 0x03)) |
+ (rtw_read8(pAdapter, EFUSE_CTRL+2) & 0xFC));
+
+ rtw_write8(pAdapter, EFUSE_CTRL+3, 0x72);/* read cmd */
+
+ while (!(0x80 & rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx < 100))
+ tmpidx++;
+ if (tmpidx < 100) {
+ *data = rtw_read8(pAdapter, EFUSE_CTRL);
+ result = true;
+ } else {
+ *data = 0xff;
+ result = false;
+ }
+ return result;
+}
+
+/* 11/16/2008 MH Write one byte to reald Efuse. */
+u8 efuse_OneByteWrite(struct adapter *pAdapter, u16 addr, u8 data, bool pseudo)
+{
+ u8 tmpidx = 0;
+ u8 result;
+
+ if (pseudo) {
+ result = Efuse_Write1ByteToFakeContent(pAdapter, addr, data);
+ return result;
+ }
+
+ /* -----------------e-fuse reg ctrl --------------------------------- */
+ /* address */
+ rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+ rtw_write8(pAdapter, EFUSE_CTRL+2,
+ (rtw_read8(pAdapter, EFUSE_CTRL+2) & 0xFC) |
+ (u8)((addr>>8) & 0x03));
+ rtw_write8(pAdapter, EFUSE_CTRL, data);/* data */
+
+ rtw_write8(pAdapter, EFUSE_CTRL+3, 0xF2);/* write cmd */
+
+ while ((0x80 & rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx < 100))
+ tmpidx++;
+
+ if (tmpidx < 100)
+ result = true;
+ else
+ result = false;
+
+ return result;
+}
+
+int Efuse_PgPacketRead(struct adapter *pAdapter, u8 offset, u8 *data, bool pseudo)
+{
+ int ret = 0;
+
+ ret = pAdapter->HalFunc.Efuse_PgPacketRead(pAdapter, offset, data, pseudo);
+
+ return ret;
+}
+
+int Efuse_PgPacketWrite(struct adapter *pAdapter, u8 offset, u8 word_en, u8 *data, bool pseudo)
+{
+ int ret;
+
+ ret = pAdapter->HalFunc.Efuse_PgPacketWrite(pAdapter, offset, word_en, data, pseudo);
+
+ return ret;
+}
+
+static int Efuse_PgPacketWrite_BT(struct adapter *pAdapter, u8 offset, u8 word_en, u8 *data, bool pseudo)
+{
+ int ret;
+
+ ret = pAdapter->HalFunc.Efuse_PgPacketWrite_BT(pAdapter, offset, word_en, data, pseudo);
+
+ return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: efuse_WordEnableDataRead
+ *
+ * Overview: Read allowed word in current efuse section data.
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/16/2008 MHC Create Version 0.
+ * 11/21/2008 MHC Fix Write bug when we only enable late word.
+ *
+ *---------------------------------------------------------------------------*/
+void efuse_WordEnableDataRead(u8 word_en, u8 *sourdata, u8 *targetdata)
+{
+ if (!(word_en&BIT(0))) {
+ targetdata[0] = sourdata[0];
+ targetdata[1] = sourdata[1];
+ }
+ if (!(word_en&BIT(1))) {
+ targetdata[2] = sourdata[2];
+ targetdata[3] = sourdata[3];
+ }
+ if (!(word_en&BIT(2))) {
+ targetdata[4] = sourdata[4];
+ targetdata[5] = sourdata[5];
+ }
+ if (!(word_en&BIT(3))) {
+ targetdata[6] = sourdata[6];
+ targetdata[7] = sourdata[7];
+ }
+}
+
+u8 Efuse_WordEnableDataWrite(struct adapter *pAdapter, u16 efuse_addr, u8 word_en, u8 *data, bool pseudo)
+{
+ u8 ret = 0;
+
+ ret = pAdapter->HalFunc.Efuse_WordEnableDataWrite(pAdapter, efuse_addr, word_en, data, pseudo);
+
+ return ret;
+}
+
+static u8 efuse_read8(struct adapter *padapter, u16 address, u8 *value)
+{
+ return efuse_OneByteRead(padapter, address, value, false);
+}
+
+static u8 efuse_write8(struct adapter *padapter, u16 address, u8 *value)
+{
+ return efuse_OneByteWrite(padapter, address, *value, false);
+}
+
+/*
+ * read/wirte raw efuse data
+ */
+u8 rtw_efuse_access(struct adapter *padapter, u8 write, u16 start_addr, u16 cnts, u8 *data)
+{
+ int i = 0;
+ u16 real_content_len = 0, max_available_size = 0;
+ u8 res = _FAIL ;
+ u8 (*rw8)(struct adapter *, u16, u8*);
+
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&real_content_len, false);
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_available_size, false);
+
+ if (start_addr > real_content_len)
+ return _FAIL;
+
+ if (write) {
+ if ((start_addr + cnts) > max_available_size)
+ return _FAIL;
+ rw8 = &efuse_write8;
+ } else {
+ rw8 = &efuse_read8;
+ }
+
+ Efuse_PowerSwitch(padapter, write, true);
+
+ /* e-fuse one byte read / write */
+ for (i = 0; i < cnts; i++) {
+ if (start_addr >= real_content_len) {
+ res = _FAIL;
+ break;
+ }
+
+ res = rw8(padapter, start_addr++, data++);
+ if (_FAIL == res)
+ break;
+ }
+
+ Efuse_PowerSwitch(padapter, write, false);
+
+ return res;
+}
+/* */
+u16 efuse_GetMaxSize(struct adapter *padapter)
+{
+ u16 max_size;
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_size, false);
+ return max_size;
+}
+/* */
+u8 efuse_GetCurrentSize(struct adapter *padapter, u16 *size)
+{
+ Efuse_PowerSwitch(padapter, false, true);
+ *size = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, false);
+ Efuse_PowerSwitch(padapter, false, false);
+
+ return _SUCCESS;
+}
+/* */
+u8 rtw_efuse_map_read(struct adapter *padapter, u16 addr, u16 cnts, u8 *data)
+{
+ u16 mapLen = 0;
+
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+ if ((addr + cnts) > mapLen)
+ return _FAIL;
+
+ Efuse_PowerSwitch(padapter, false, true);
+
+ efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data, false);
+
+ Efuse_PowerSwitch(padapter, false, false);
+
+ return _SUCCESS;
+}
+
+u8 rtw_BT_efuse_map_read(struct adapter *padapter, u16 addr, u16 cnts, u8 *data)
+{
+ u16 mapLen = 0;
+
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+ if ((addr + cnts) > mapLen)
+ return _FAIL;
+
+ Efuse_PowerSwitch(padapter, false, true);
+
+ efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data, false);
+
+ Efuse_PowerSwitch(padapter, false, false);
+
+ return _SUCCESS;
+}
+/* */
+u8 rtw_efuse_map_write(struct adapter *padapter, u16 addr, u16 cnts, u8 *data)
+{
+ u8 offset, word_en;
+ u8 *map;
+ u8 newdata[PGPKT_DATA_SIZE + 1];
+ s32 i, idx;
+ u8 ret = _SUCCESS;
+ u16 mapLen = 0;
+
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+ if ((addr + cnts) > mapLen)
+ return _FAIL;
+
+ map = rtw_zmalloc(mapLen);
+ if (map == NULL)
+ return _FAIL;
+
+ ret = rtw_efuse_map_read(padapter, 0, mapLen, map);
+ if (ret == _FAIL)
+ goto exit;
+
+ Efuse_PowerSwitch(padapter, true, true);
+
+ offset = (addr >> 3);
+ word_en = 0xF;
+ memset(newdata, 0xFF, PGPKT_DATA_SIZE + 1);
+ i = addr & 0x7; /* index of one package */
+ idx = 0; /* data index */
+
+ if (i & 0x1) {
+ /* odd start */
+ if (data[idx] != map[addr+idx]) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i-1] = map[addr+idx-1];
+ newdata[i] = data[idx];
+ }
+ i++;
+ idx++;
+ }
+ do {
+ for (; i < PGPKT_DATA_SIZE; i += 2) {
+ if (cnts == idx)
+ break;
+ if ((cnts - idx) == 1) {
+ if (data[idx] != map[addr+idx]) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i] = data[idx];
+ newdata[i+1] = map[addr+idx+1];
+ }
+ idx++;
+ break;
+ } else {
+ if ((data[idx] != map[addr+idx]) ||
+ (data[idx+1] != map[addr+idx+1])) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i] = data[idx];
+ newdata[i+1] = data[idx + 1];
+ }
+ idx += 2;
+ }
+ if (idx == cnts)
+ break;
+ }
+
+ if (word_en != 0xF) {
+ ret = Efuse_PgPacketWrite(padapter, offset, word_en, newdata, false);
+ DBG_88E("offset=%x\n", offset);
+ DBG_88E("word_en=%x\n", word_en);
+
+ for (i = 0; i < PGPKT_DATA_SIZE; i++)
+ DBG_88E("data=%x \t", newdata[i]);
+ if (ret == _FAIL)
+ break;
+ }
+
+ if (idx == cnts)
+ break;
+
+ offset++;
+ i = 0;
+ word_en = 0xF;
+ memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+ } while (1);
+
+ Efuse_PowerSwitch(padapter, true, false);
+exit:
+ kfree(map);
+ return ret;
+}
+
+/* */
+u8 rtw_BT_efuse_map_write(struct adapter *padapter, u16 addr, u16 cnts, u8 *data)
+{
+ u8 offset, word_en;
+ u8 *map;
+ u8 newdata[PGPKT_DATA_SIZE + 1];
+ s32 i, idx;
+ u8 ret = _SUCCESS;
+ u16 mapLen = 0;
+
+ EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+ if ((addr + cnts) > mapLen)
+ return _FAIL;
+
+ map = rtw_zmalloc(mapLen);
+ if (map == NULL)
+ return _FAIL;
+
+ ret = rtw_BT_efuse_map_read(padapter, 0, mapLen, map);
+ if (ret == _FAIL)
+ goto exit;
+
+ Efuse_PowerSwitch(padapter, true, true);
+
+ offset = (addr >> 3);
+ word_en = 0xF;
+ memset(newdata, 0xFF, PGPKT_DATA_SIZE + 1);
+ i = addr & 0x7; /* index of one package */
+ idx = 0; /* data index */
+
+ if (i & 0x1) {
+ /* odd start */
+ if (data[idx] != map[addr+idx]) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i-1] = map[addr+idx-1];
+ newdata[i] = data[idx];
+ }
+ i++;
+ idx++;
+ }
+ do {
+ for (; i < PGPKT_DATA_SIZE; i += 2) {
+ if (cnts == idx)
+ break;
+ if ((cnts - idx) == 1) {
+ if (data[idx] != map[addr+idx]) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i] = data[idx];
+ newdata[i+1] = map[addr+idx+1];
+ }
+ idx++;
+ break;
+ } else {
+ if ((data[idx] != map[addr+idx]) ||
+ (data[idx+1] != map[addr+idx+1])) {
+ word_en &= ~BIT(i >> 1);
+ newdata[i] = data[idx];
+ newdata[i+1] = data[idx + 1];
+ }
+ idx += 2;
+ }
+ if (idx == cnts)
+ break;
+ }
+
+ if (word_en != 0xF) {
+ DBG_88E("%s: offset=%#X\n", __func__, offset);
+ DBG_88E("%s: word_en=%#X\n", __func__, word_en);
+ DBG_88E("%s: data=", __func__);
+ for (i = 0; i < PGPKT_DATA_SIZE; i++)
+ DBG_88E("0x%02X ", newdata[i]);
+ DBG_88E("\n");
+
+ ret = Efuse_PgPacketWrite_BT(padapter, offset, word_en, newdata, false);
+ if (ret == _FAIL)
+ break;
+ }
+
+ if (idx == cnts)
+ break;
+
+ offset++;
+ i = 0;
+ word_en = 0xF;
+ memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+ } while (1);
+
+ Efuse_PowerSwitch(padapter, true, false);
+
+exit:
+
+ kfree(map);
+
+ return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: efuse_ShadowRead1Byte
+ * efuse_ShadowRead2Byte
+ * efuse_ShadowRead4Byte
+ *
+ * Overview: Read from efuse init map by one/two/four bytes !!!!!
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/12/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static void
+efuse_ShadowRead1Byte(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u8 *Value)
+{
+ struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+ *Value = pEEPROM->efuse_eeprom_data[Offset];
+
+} /* EFUSE_ShadowRead1Byte */
+
+/* Read Two Bytes */
+static void
+efuse_ShadowRead2Byte(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u16 *Value)
+{
+ struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+ *Value = pEEPROM->efuse_eeprom_data[Offset];
+ *Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+
+} /* EFUSE_ShadowRead2Byte */
+
+/* Read Four Bytes */
+static void
+efuse_ShadowRead4Byte(
+ struct adapter *pAdapter,
+ u16 Offset,
+ u32 *Value)
+{
+ struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+ *Value = pEEPROM->efuse_eeprom_data[Offset];
+ *Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+ *Value |= pEEPROM->efuse_eeprom_data[Offset+2]<<16;
+ *Value |= pEEPROM->efuse_eeprom_data[Offset+3]<<24;
+
+} /* efuse_ShadowRead4Byte */
+
+/*-----------------------------------------------------------------------------
+ * Function: Efuse_ReadAllMap
+ *
+ * Overview: Read All Efuse content
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/11/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static void Efuse_ReadAllMap(struct adapter *pAdapter, u8 efuseType, u8 *Efuse, bool pseudo)
+{
+ u16 mapLen = 0;
+
+ Efuse_PowerSwitch(pAdapter, false, true);
+
+ EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, pseudo);
+
+ efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse, pseudo);
+
+ Efuse_PowerSwitch(pAdapter, false, false);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: EFUSE_ShadowMapUpdate
+ *
+ * Overview: Transfer current EFUSE content to shadow init and modify map.
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/13/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowMapUpdate(
+ struct adapter *pAdapter,
+ u8 efuseType,
+ bool pseudo)
+{
+ struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+ u16 mapLen = 0;
+
+ EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, pseudo);
+
+ if (pEEPROM->bautoload_fail_flag)
+ memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen);
+ else
+ Efuse_ReadAllMap(pAdapter, efuseType, pEEPROM->efuse_eeprom_data, pseudo);
+} /* EFUSE_ShadowMapUpdate */
+
+/*-----------------------------------------------------------------------------
+ * Function: EFUSE_ShadowRead
+ *
+ * Overview: Read from efuse init map !!!!!
+ *
+ * Input: NONE
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 11/12/2008 MHC Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowRead(struct adapter *pAdapter, u8 Type, u16 Offset, u32 *Value)
+{
+ if (Type == 1)
+ efuse_ShadowRead1Byte(pAdapter, Offset, (u8 *)Value);
+ else if (Type == 2)
+ efuse_ShadowRead2Byte(pAdapter, Offset, (u16 *)Value);
+ else if (Type == 4)
+ efuse_ShadowRead4Byte(pAdapter, Offset, (u32 *)Value);
+
+} /* EFUSE_ShadowRead */
diff --git a/drivers/staging/r8188eu/core/rtw_ieee80211.c b/drivers/staging/r8188eu/core/rtw_ieee80211.c
new file mode 100644
index 000000000000..7159ce4c1acd
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ieee80211.c
@@ -0,0 +1,1625 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _IEEE80211_C
+
+#include <drv_types.h>
+#include <ieee80211.h>
+#include <wifi.h>
+#include <osdep_service.h>
+#include <wlan_bssdef.h>
+#include <usb_osintf.h>
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+/* */
+/* for adhoc-master to generate ie and provide supported-rate to fw */
+/* */
+
+static u8 WIFI_CCKRATES[] = {
+ (IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+ };
+
+static u8 WIFI_OFDMRATES[] = {
+ (IEEE80211_OFDM_RATE_6MB),
+ (IEEE80211_OFDM_RATE_9MB),
+ (IEEE80211_OFDM_RATE_12MB),
+ (IEEE80211_OFDM_RATE_18MB),
+ (IEEE80211_OFDM_RATE_24MB),
+ IEEE80211_OFDM_RATE_36MB,
+ IEEE80211_OFDM_RATE_48MB,
+ IEEE80211_OFDM_RATE_54MB
+ };
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+ unsigned char dot11_rate_table[] = {
+ 2, 4, 11, 22, 12, 18, 24, 36, 48,
+ 72, 96, 108, 0}; /* last element must be zero!! */
+
+ int i = 0;
+ while (dot11_rate_table[i] != 0) {
+ if (dot11_rate_table[i] == val)
+ return BIT(i);
+ i++;
+ }
+ return 0;
+}
+
+uint rtw_is_cckrates_included(u8 *rate)
+{
+ u32 i = 0;
+
+ while (rate[i] != 0) {
+ if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
+ (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
+ return true;
+ i++;
+ }
+ return false;
+}
+
+uint rtw_is_cckratesonly_included(u8 *rate)
+{
+ u32 i = 0;
+
+ while (rate[i] != 0) {
+ if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+ (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22))
+ return false;
+ i++;
+ }
+
+ return true;
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+ if (channel > 14) {
+ if ((rtw_is_cckrates_included(rate)) == true)
+ return WIRELESS_INVALID;
+ else
+ return WIRELESS_11A;
+ } else { /* could be pure B, pure G, or B/G */
+ if ((rtw_is_cckratesonly_included(rate)) == true)
+ return WIRELESS_11B;
+ else if ((rtw_is_cckrates_included(rate)) == true)
+ return WIRELESS_11BG;
+ else
+ return WIRELESS_11G;
+ }
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+ unsigned int *frlen)
+{
+ memcpy((void *)pbuf, (void *)source, len);
+ *frlen = *frlen + len;
+ return pbuf + len;
+}
+
+/* rtw_set_ie will update frame length */
+u8 *rtw_set_ie
+(
+ u8 *pbuf,
+ int index,
+ uint len,
+ u8 *source,
+ uint *frlen /* frame length */
+)
+{
+
+ *pbuf = (u8)index;
+
+ *(pbuf + 1) = (u8)len;
+
+ if (len > 0)
+ memcpy((void *)(pbuf + 2), (void *)source, len);
+
+ *frlen = *frlen + (len + 2);
+
+ return pbuf + len + 2;
+}
+
+inline u8 *rtw_set_ie_ch_switch (u8 *buf, u32 *buf_len, u8 ch_switch_mode,
+ u8 new_ch, u8 ch_switch_cnt)
+{
+ u8 ie_data[3];
+
+ ie_data[0] = ch_switch_mode;
+ ie_data[1] = new_ch;
+ ie_data[2] = ch_switch_cnt;
+ return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH, 3, ie_data, buf_len);
+}
+
+inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
+{
+ if (ch_offset == SCN)
+ return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ else if (ch_offset == SCA)
+ return HAL_PRIME_CHNL_OFFSET_UPPER;
+ else if (ch_offset == SCB)
+ return HAL_PRIME_CHNL_OFFSET_LOWER;
+
+ return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+}
+
+inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
+{
+ if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
+ return SCN;
+ else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+ return SCB;
+ else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+ return SCA;
+
+ return SCN;
+}
+
+inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
+{
+ return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET, 1, &secondary_ch_offset, buf_len);
+}
+
+inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
+ u8 flags, u16 reason, u16 precedence)
+{
+ u8 ie_data[6];
+
+ ie_data[0] = ttl;
+ ie_data[1] = flags;
+ *(u16 *)(ie_data+2) = cpu_to_le16(reason);
+ *(u16 *)(ie_data+4) = cpu_to_le16(precedence);
+
+ return rtw_set_ie(buf, 0x118, 6, ie_data, buf_len);
+}
+
+/*----------------------------------------------------------------------------
+index: the information element id index, limit is the limit for search
+-----------------------------------------------------------------------------*/
+u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit)
+{
+ int tmp, i;
+ u8 *p;
+
+ if (limit < 1) {
+
+ return NULL;
+ }
+
+ p = pbuf;
+ i = 0;
+ *len = 0;
+ while (1) {
+ if (*p == index) {
+ *len = *(p + 1);
+ return p;
+ } else {
+ tmp = *(p + 1);
+ p += (tmp + 2);
+ i += (tmp + 2);
+ }
+ if (i >= limit)
+ break;
+ }
+
+ return NULL;
+}
+
+/**
+ * rtw_get_ie_ex - Search specific IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
+ * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
+ *
+ * Returns: The address of the specific IE found, or NULL
+ */
+u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
+{
+ uint cnt;
+ u8 *target_ie = NULL;
+
+ if (ielen)
+ *ielen = 0;
+
+ if (!in_ie || in_len <= 0)
+ return target_ie;
+
+ cnt = 0;
+
+ while (cnt < in_len) {
+ if (eid == in_ie[cnt] && (!oui || !memcmp(&in_ie[cnt+2], oui, oui_len))) {
+ target_ie = &in_ie[cnt];
+
+ if (ie)
+ memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+ if (ielen)
+ *ielen = in_ie[cnt+1]+2;
+
+ break;
+ } else {
+ cnt += in_ie[cnt+1]+2; /* goto next */
+ }
+ }
+ return target_ie;
+}
+
+/**
+ * rtw_ies_remove_ie - Find matching IEs and remove
+ * @ies: Address of IEs to search
+ * @ies_len: Pointer of length of ies, will update to new length
+ * @offset: The offset to start scarch
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ *
+ * Returns: _SUCCESS: ies is updated, _FAIL: not updated
+ */
+int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
+{
+ int ret = _FAIL;
+ u8 *target_ie;
+ u32 target_ielen;
+ u8 *start;
+ uint search_len;
+
+ if (!ies || !ies_len || *ies_len <= offset)
+ goto exit;
+
+ start = ies + offset;
+ search_len = *ies_len - offset;
+
+ while (1) {
+ target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
+ if (target_ie && target_ielen) {
+ u8 buf[MAX_IE_SZ] = {0};
+ u8 *remain_ies = target_ie + target_ielen;
+ uint remain_len = search_len - (remain_ies - start);
+
+ memcpy(buf, remain_ies, remain_len);
+ memcpy(target_ie, buf, remain_len);
+ *ies_len = *ies_len - target_ielen;
+ ret = _SUCCESS;
+
+ start = target_ie;
+ search_len = remain_len;
+ } else {
+ break;
+ }
+ }
+exit:
+ return ret;
+}
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
+{
+
+ memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+ switch (mode) {
+ case WIRELESS_11B:
+ memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+ break;
+ case WIRELESS_11G:
+ case WIRELESS_11A:
+ case WIRELESS_11_5N:
+ case WIRELESS_11A_5N:/* Todo: no basic rate for ofdm ? */
+ memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+ break;
+ case WIRELESS_11BG:
+ case WIRELESS_11G_24N:
+ case WIRELESS_11_24N:
+ case WIRELESS_11BG_24N:
+ memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+ memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+ break;
+ }
+
+}
+
+uint rtw_get_rateset_len(u8 *rateset)
+{
+ uint i = 0;
+
+ while (1) {
+ if ((rateset[i]) == 0)
+ break;
+ if (i > 12)
+ break;
+ i++;
+ }
+
+ return i;
+}
+
+int rtw_generate_ie(struct registry_priv *pregistrypriv)
+{
+ u8 wireless_mode;
+ int sz = 0, rateLen;
+ struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+ u8 *ie = pdev_network->IEs;
+
+ /* timestamp will be inserted by hardware */
+ sz += 8;
+ ie += sz;
+
+ /* beacon interval : 2bytes */
+ *(__le16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);/* BCN_INTERVAL; */
+ sz += 2;
+ ie += 2;
+
+ /* capability info */
+ *(u16 *)ie = 0;
+
+ *(__le16 *)ie |= cpu_to_le16(cap_IBSS);
+
+ if (pregistrypriv->preamble == PREAMBLE_SHORT)
+ *(__le16 *)ie |= cpu_to_le16(cap_ShortPremble);
+
+ if (pdev_network->Privacy)
+ *(__le16 *)ie |= cpu_to_le16(cap_Privacy);
+
+ sz += 2;
+ ie += 2;
+
+ /* SSID */
+ ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
+
+ /* supported rates */
+ if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
+ if (pdev_network->Configuration.DSConfig > 14)
+ wireless_mode = WIRELESS_11A_5N;
+ else
+ wireless_mode = WIRELESS_11BG_24N;
+ } else {
+ wireless_mode = pregistrypriv->wireless_mode;
+ }
+
+ rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode);
+
+ rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
+
+ if (rateLen > 8) {
+ ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
+ /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
+ } else {
+ ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
+ }
+
+ /* DS parameter set */
+ ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
+
+ /* IBSS Parameter Set */
+
+ ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
+
+ if (rateLen > 8)
+ ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
+
+ return sz;
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+ int len;
+ u16 val16;
+ __le16 le_tmp;
+ unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+ u8 *pbuf = pie;
+ int limit_new = limit;
+
+ while (1) {
+ pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
+
+ if (pbuf) {
+ /* check if oui matches... */
+ if (memcmp((pbuf + 2), wpa_oui_type, sizeof (wpa_oui_type)))
+ goto check_next_ie;
+
+ /* check version... */
+ memcpy((u8 *)&le_tmp, (pbuf + 6), sizeof(val16));
+
+ val16 = le16_to_cpu(le_tmp);
+ if (val16 != 0x0001)
+ goto check_next_ie;
+ *wpa_ie_len = *(pbuf + 1);
+ return pbuf;
+ } else {
+ *wpa_ie_len = 0;
+ return NULL;
+ }
+
+check_next_ie:
+ limit_new = limit - (pbuf - pie) - 2 - len;
+ if (limit_new <= 0)
+ break;
+ pbuf += (2 + len);
+ }
+ *wpa_ie_len = 0;
+ return NULL;
+}
+
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+{
+
+ return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
+}
+
+int rtw_get_wpa_cipher_suite(u8 *s)
+{
+ if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
+ return WPA_CIPHER_NONE;
+ if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
+ return WPA_CIPHER_WEP40;
+ if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
+ return WPA_CIPHER_TKIP;
+ if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
+ return WPA_CIPHER_CCMP;
+ if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
+ return WPA_CIPHER_WEP104;
+
+ return 0;
+}
+
+int rtw_get_wpa2_cipher_suite(u8 *s)
+{
+ if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
+ return WPA_CIPHER_NONE;
+ if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
+ return WPA_CIPHER_WEP40;
+ if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
+ return WPA_CIPHER_TKIP;
+ if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
+ return WPA_CIPHER_CCMP;
+ if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
+ return WPA_CIPHER_WEP104;
+
+ return 0;
+}
+
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+ int i, ret = _SUCCESS;
+ int left, count;
+ u8 *pos;
+ u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+
+ if (wpa_ie_len <= 0) {
+ /* No WPA IE - fail silently */
+ return _FAIL;
+ }
+
+ if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
+ (memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
+ return _FAIL;
+
+ pos = wpa_ie;
+
+ pos += 8;
+ left = wpa_ie_len - 8;
+
+ /* group_cipher */
+ if (left >= WPA_SELECTOR_LEN) {
+ *group_cipher = rtw_get_wpa_cipher_suite(pos);
+ pos += WPA_SELECTOR_LEN;
+ left -= WPA_SELECTOR_LEN;
+ } else if (left > 0) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+ return _FAIL;
+ }
+
+ /* pairwise_cipher */
+ if (left >= 2) {
+ count = get_unaligned_le16(pos);
+ pos += 2;
+ left -= 2;
+
+ if (count == 0 || left < count * WPA_SELECTOR_LEN) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+ "count %u left %u", __func__, count, left));
+ return _FAIL;
+ }
+
+ for (i = 0; i < count; i++) {
+ *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+
+ pos += WPA_SELECTOR_LEN;
+ left -= WPA_SELECTOR_LEN;
+ }
+ } else if (left == 1) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)", __func__));
+ return _FAIL;
+ }
+
+ if (is_8021x) {
+ if (left >= 6) {
+ pos += 2;
+ if (!memcmp(pos, SUITE_1X, 4)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s : there has 802.1x auth\n", __func__));
+ *is_8021x = 1;
+ }
+ }
+ }
+
+ return ret;
+}
+
+int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+ int i, ret = _SUCCESS;
+ int left, count;
+ u8 *pos;
+ u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
+
+ if (rsn_ie_len <= 0) {
+ /* No RSN IE - fail silently */
+ return _FAIL;
+ }
+
+ if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
+ return _FAIL;
+
+ pos = rsn_ie;
+ pos += 4;
+ left = rsn_ie_len - 4;
+
+ /* group_cipher */
+ if (left >= RSN_SELECTOR_LEN) {
+ *group_cipher = rtw_get_wpa2_cipher_suite(pos);
+
+ pos += RSN_SELECTOR_LEN;
+ left -= RSN_SELECTOR_LEN;
+
+ } else if (left > 0) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+ return _FAIL;
+ }
+
+ /* pairwise_cipher */
+ if (left >= 2) {
+ count = get_unaligned_le16(pos);
+ pos += 2;
+ left -= 2;
+
+ if (count == 0 || left < count * RSN_SELECTOR_LEN) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+ "count %u left %u", __func__, count, left));
+ return _FAIL;
+ }
+
+ for (i = 0; i < count; i++) {
+ *pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+
+ pos += RSN_SELECTOR_LEN;
+ left -= RSN_SELECTOR_LEN;
+ }
+
+ } else if (left == 1) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)", __func__));
+
+ return _FAIL;
+ }
+
+ if (is_8021x) {
+ if (left >= 6) {
+ pos += 2;
+ if (!memcmp(pos, SUITE_1X, 4)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__));
+ *is_8021x = 1;
+ }
+ }
+ }
+ return ret;
+}
+
+int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
+{
+ u8 authmode, sec_idx, i;
+ u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+ uint cnt;
+
+ /* Search required WPA or WPA2 IE and copy to sec_ie[] */
+
+ cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+ sec_idx = 0;
+
+ while (cnt < in_len) {
+ authmode = in_ie[cnt];
+
+ if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], &wpa_oui[0], 4))) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("\n rtw_get_wpa_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n",
+ sec_idx, in_ie[cnt+1]+2));
+
+ if (wpa_ie) {
+ memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+ for (i = 0; i < (in_ie[cnt+1]+2); i += 8) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+ wpa_ie[i], wpa_ie[i+1], wpa_ie[i+2], wpa_ie[i+3], wpa_ie[i+4],
+ wpa_ie[i+5], wpa_ie[i+6], wpa_ie[i+7]));
+ }
+ }
+
+ *wpa_len = in_ie[cnt+1]+2;
+ cnt += in_ie[cnt+1]+2; /* get next */
+ } else {
+ if (authmode == _WPA2_IE_ID_) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("\n get_rsn_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n",
+ sec_idx, in_ie[cnt+1]+2));
+
+ if (rsn_ie) {
+ memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+ for (i = 0; i < (in_ie[cnt+1]+2); i += 8) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+ rsn_ie[i], rsn_ie[i+1], rsn_ie[i+2], rsn_ie[i+3], rsn_ie[i+4],
+ rsn_ie[i+5], rsn_ie[i+6], rsn_ie[i+7]));
+ }
+ }
+
+ *rsn_len = in_ie[cnt+1]+2;
+ cnt += in_ie[cnt+1]+2; /* get next */
+ } else {
+ cnt += in_ie[cnt+1]+2; /* get next */
+ }
+ }
+ }
+
+ return *rsn_len + *wpa_len;
+}
+
+u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
+{
+ u8 match = false;
+ u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+ if (ie_ptr == NULL)
+ return match;
+
+ eid = ie_ptr[0];
+
+ if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) {
+ *wps_ielen = ie_ptr[1]+2;
+ match = true;
+ }
+ return match;
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+ uint cnt;
+ u8 *wpsie_ptr = NULL;
+ u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+ if (wps_ielen)
+ *wps_ielen = 0;
+
+ if (!in_ie || in_len <= 0)
+ return wpsie_ptr;
+
+ cnt = 0;
+
+ while (cnt < in_len) {
+ eid = in_ie[cnt];
+
+ if ((eid == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], wps_oui, 4))) {
+ wpsie_ptr = &in_ie[cnt];
+
+ if (wps_ie)
+ memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+ if (wps_ielen)
+ *wps_ielen = in_ie[cnt+1]+2;
+
+ cnt += in_ie[cnt+1]+2;
+
+ break;
+ } else {
+ cnt += in_ie[cnt+1]+2; /* goto next */
+ }
+ }
+ return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
+{
+ u8 *attr_ptr = NULL;
+ u8 *target_attr_ptr = NULL;
+ u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
+
+ if (len_attr)
+ *len_attr = 0;
+
+ if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+ (memcmp(wps_ie + 2, wps_oui , 4)))
+ return attr_ptr;
+
+ /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
+ attr_ptr = wps_ie + 6; /* goto first attr */
+
+ while (attr_ptr - wps_ie < wps_ielen) {
+ /* 4 = 2(Attribute ID) + 2(Length) */
+ u16 attr_id = RTW_GET_BE16(attr_ptr);
+ u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
+ u16 attr_len = attr_data_len + 4;
+
+ if (attr_id == target_attr_id) {
+ target_attr_ptr = attr_ptr;
+ if (buf_attr)
+ memcpy(buf_attr, attr_ptr, attr_len);
+ if (len_attr)
+ *len_attr = attr_len;
+ break;
+ } else {
+ attr_ptr += attr_len; /* goto next */
+ }
+ }
+ return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
+ *
+ * Returns: the address of the specific WPS attribute content found, or NULL
+ */
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content)
+{
+ u8 *attr_ptr;
+ u32 attr_len;
+
+ if (len_content)
+ *len_content = 0;
+
+ attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
+
+ if (attr_ptr && attr_len) {
+ if (buf_content)
+ memcpy(buf_content, attr_ptr+4, attr_len-4);
+
+ if (len_content)
+ *len_content = attr_len-4;
+
+ return attr_ptr+4;
+ }
+
+ return NULL;
+}
+
+static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
+ struct rtw_ieee802_11_elems *elems,
+ int show_errors)
+{
+ unsigned int oui;
+
+ /* first 3 bytes in vendor specific information element are the IEEE
+ * OUI of the vendor. The following byte is used a vendor specific
+ * sub-type. */
+ if (elen < 4) {
+ if (show_errors) {
+ DBG_88E("short vendor specific information element ignored (len=%lu)\n",
+ (unsigned long) elen);
+ }
+ return -1;
+ }
+
+ oui = RTW_GET_BE24(pos);
+ switch (oui) {
+ case OUI_MICROSOFT:
+ /* Microsoft/Wi-Fi information elements are further typed and
+ * subtyped */
+ switch (pos[3]) {
+ case 1:
+ /* Microsoft OUI (00:50:F2) with OUI Type 1:
+ * real WPA information element */
+ elems->wpa_ie = pos;
+ elems->wpa_ie_len = elen;
+ break;
+ case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+ if (elen < 5) {
+ DBG_88E("short WME information element ignored (len=%lu)\n",
+ (unsigned long) elen);
+ return -1;
+ }
+ switch (pos[4]) {
+ case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+ case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+ elems->wme = pos;
+ elems->wme_len = elen;
+ break;
+ case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+ elems->wme_tspec = pos;
+ elems->wme_tspec_len = elen;
+ break;
+ default:
+ DBG_88E("unknown WME information element ignored (subtype=%d len=%lu)\n",
+ pos[4], (unsigned long) elen);
+ return -1;
+ }
+ break;
+ case 4:
+ /* Wi-Fi Protected Setup (WPS) IE */
+ elems->wps_ie = pos;
+ elems->wps_ie_len = elen;
+ break;
+ default:
+ DBG_88E("Unknown Microsoft information element ignored (type=%d len=%lu)\n",
+ pos[3], (unsigned long) elen);
+ return -1;
+ }
+ break;
+
+ case OUI_BROADCOM:
+ switch (pos[3]) {
+ case VENDOR_HT_CAPAB_OUI_TYPE:
+ elems->vendor_ht_cap = pos;
+ elems->vendor_ht_cap_len = elen;
+ break;
+ default:
+ DBG_88E("Unknown Broadcom information element ignored (type=%d len=%lu)\n",
+ pos[3], (unsigned long) elen);
+ return -1;
+ }
+ break;
+ default:
+ DBG_88E("unknown vendor specific information element ignored (vendor OUI %02x:%02x:%02x len=%lu)\n",
+ pos[0], pos[1], pos[2], (unsigned long) elen);
+ return -1;
+ }
+ return 0;
+}
+
+/**
+ * ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+enum parse_res rtw_ieee802_11_parse_elems(u8 *start, uint len,
+ struct rtw_ieee802_11_elems *elems,
+ int show_errors)
+{
+ uint left = len;
+ u8 *pos = start;
+ int unknown = 0;
+
+ memset(elems, 0, sizeof(*elems));
+
+ while (left >= 2) {
+ u8 id, elen;
+
+ id = *pos++;
+ elen = *pos++;
+ left -= 2;
+
+ if (elen > left) {
+ if (show_errors) {
+ DBG_88E("IEEE 802.11 element parse failed (id=%d elen=%d left=%lu)\n",
+ id, elen, (unsigned long) left);
+ }
+ return ParseFailed;
+ }
+
+ switch (id) {
+ case WLAN_EID_SSID:
+ elems->ssid = pos;
+ elems->ssid_len = elen;
+ break;
+ case WLAN_EID_SUPP_RATES:
+ elems->supp_rates = pos;
+ elems->supp_rates_len = elen;
+ break;
+ case WLAN_EID_FH_PARAMS:
+ elems->fh_params = pos;
+ elems->fh_params_len = elen;
+ break;
+ case WLAN_EID_DS_PARAMS:
+ elems->ds_params = pos;
+ elems->ds_params_len = elen;
+ break;
+ case WLAN_EID_CF_PARAMS:
+ elems->cf_params = pos;
+ elems->cf_params_len = elen;
+ break;
+ case WLAN_EID_TIM:
+ elems->tim = pos;
+ elems->tim_len = elen;
+ break;
+ case WLAN_EID_IBSS_PARAMS:
+ elems->ibss_params = pos;
+ elems->ibss_params_len = elen;
+ break;
+ case WLAN_EID_CHALLENGE:
+ elems->challenge = pos;
+ elems->challenge_len = elen;
+ break;
+ case WLAN_EID_ERP_INFO:
+ elems->erp_info = pos;
+ elems->erp_info_len = elen;
+ break;
+ case WLAN_EID_EXT_SUPP_RATES:
+ elems->ext_supp_rates = pos;
+ elems->ext_supp_rates_len = elen;
+ break;
+ case WLAN_EID_VENDOR_SPECIFIC:
+ if (rtw_ieee802_11_parse_vendor_specific(pos, elen, elems, show_errors))
+ unknown++;
+ break;
+ case WLAN_EID_RSN:
+ elems->rsn_ie = pos;
+ elems->rsn_ie_len = elen;
+ break;
+ case WLAN_EID_PWR_CAPABILITY:
+ elems->power_cap = pos;
+ elems->power_cap_len = elen;
+ break;
+ case WLAN_EID_SUPPORTED_CHANNELS:
+ elems->supp_channels = pos;
+ elems->supp_channels_len = elen;
+ break;
+ case WLAN_EID_MOBILITY_DOMAIN:
+ elems->mdie = pos;
+ elems->mdie_len = elen;
+ break;
+ case WLAN_EID_FAST_BSS_TRANSITION:
+ elems->ftie = pos;
+ elems->ftie_len = elen;
+ break;
+ case WLAN_EID_TIMEOUT_INTERVAL:
+ elems->timeout_int = pos;
+ elems->timeout_int_len = elen;
+ break;
+ case WLAN_EID_HT_CAP:
+ elems->ht_capabilities = pos;
+ elems->ht_capabilities_len = elen;
+ break;
+ case WLAN_EID_HT_OPERATION:
+ elems->ht_operation = pos;
+ elems->ht_operation_len = elen;
+ break;
+ default:
+ unknown++;
+ if (!show_errors)
+ break;
+ DBG_88E("IEEE 802.11 element parse ignored unknown element (id=%d elen=%d)\n",
+ id, elen);
+ break;
+ }
+ left -= elen;
+ pos += elen;
+ }
+ if (left)
+ return ParseFailed;
+ return unknown ? ParseUnknown : ParseOK;
+}
+
+u8 key_char2num(u8 ch)
+{
+ if ((ch >= '0') && (ch <= '9'))
+ return ch - '0';
+ else if ((ch >= 'a') && (ch <= 'f'))
+ return ch - 'a' + 10;
+ else if ((ch >= 'A') && (ch <= 'F'))
+ return ch - 'A' + 10;
+ else
+ return 0xff;
+}
+
+u8 str_2char2num(u8 hch, u8 lch)
+{
+ return (key_char2num(hch) * 10) + key_char2num(lch);
+}
+
+u8 key_2char2num(u8 hch, u8 lch)
+{
+ return (key_char2num(hch) << 4) | key_char2num(lch);
+}
+
+void rtw_macaddr_cfg(u8 *mac_addr)
+{
+ u8 mac[ETH_ALEN];
+ if (mac_addr == NULL)
+ return;
+
+ if (rtw_initmac) { /* Users specify the mac address */
+ int jj, kk;
+
+ for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+ mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
+ memcpy(mac_addr, mac, ETH_ALEN);
+ } else { /* Use the mac address stored in the Efuse */
+ memcpy(mac, mac_addr, ETH_ALEN);
+ }
+
+ if (((mac[0] == 0xff) && (mac[1] == 0xff) && (mac[2] == 0xff) &&
+ (mac[3] == 0xff) && (mac[4] == 0xff) && (mac[5] == 0xff)) ||
+ ((mac[0] == 0x0) && (mac[1] == 0x0) && (mac[2] == 0x0) &&
+ (mac[3] == 0x0) && (mac[4] == 0x0) && (mac[5] == 0x0))) {
+ mac[0] = 0x00;
+ mac[1] = 0xe0;
+ mac[2] = 0x4c;
+ mac[3] = 0x87;
+ mac[4] = 0x00;
+ mac[5] = 0x00;
+ /* use default mac addresss */
+ memcpy(mac_addr, mac, ETH_ALEN);
+ DBG_88E("MAC Address from efuse error, assign default one !!!\n");
+ }
+
+ DBG_88E("rtw_macaddr_cfg MAC Address = %pM\n", (mac_addr));
+}
+
+void dump_ies(u8 *buf, u32 buf_len)
+{
+ u8 *pos = (u8 *)buf;
+ u8 id, len;
+
+ while (pos-buf <= buf_len) {
+ id = *pos;
+ len = *(pos+1);
+
+ DBG_88E("%s ID:%u, LEN:%u\n", __func__, id, len);
+ #ifdef CONFIG_88EU_P2P
+ dump_p2p_ie(pos, len);
+ #endif
+ dump_wps_ie(pos, len);
+
+ pos += (2 + len);
+ }
+}
+
+void dump_wps_ie(u8 *ie, u32 ie_len)
+{
+ u8 *pos = (u8 *)ie;
+ u16 id;
+ u16 len;
+ u8 *wps_ie;
+ uint wps_ielen;
+
+ wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
+ if (wps_ie != ie || wps_ielen == 0)
+ return;
+
+ pos += 6;
+ while (pos-ie < ie_len) {
+ id = RTW_GET_BE16(pos);
+ len = RTW_GET_BE16(pos + 2);
+ DBG_88E("%s ID:0x%04x, LEN:%u\n", __func__, id, len);
+ pos += (4+len);
+ }
+}
+
+#ifdef CONFIG_88EU_P2P
+void dump_p2p_ie(u8 *ie, u32 ie_len)
+{
+ u8 *pos = (u8 *)ie;
+ u8 id;
+ u16 len;
+ u8 *p2p_ie;
+ uint p2p_ielen;
+
+ p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
+ if (p2p_ie != ie || p2p_ielen == 0)
+ return;
+
+ pos += 6;
+ while (pos-ie < ie_len) {
+ id = *pos;
+ len = get_unaligned_le16(pos+1);
+ DBG_88E("%s ID:%u, LEN:%u\n", __func__, id, len);
+ pos += (3+len);
+ }
+}
+
+/**
+ * rtw_get_p2p_ie - Search P2P IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
+ * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
+ *
+ * Returns: The address of the P2P IE found, or NULL
+ */
+u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
+{
+ uint cnt = 0;
+ u8 *p2p_ie_ptr;
+ u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+ if (p2p_ielen != NULL)
+ *p2p_ielen = 0;
+
+ while (cnt < in_len) {
+ eid = in_ie[cnt];
+ if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
+ dump_stack();
+ return NULL;
+ }
+ if ((eid == _VENDOR_SPECIFIC_IE_) && !memcmp(&in_ie[cnt+2], p2p_oui, 4)) {
+ p2p_ie_ptr = in_ie + cnt;
+
+ if (p2p_ie != NULL)
+ memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+ if (p2p_ielen != NULL)
+ *p2p_ielen = in_ie[cnt + 1] + 2;
+ return p2p_ie_ptr;
+ } else {
+ cnt += in_ie[cnt + 1] + 2; /* goto next */
+ }
+ }
+ return NULL;
+}
+
+/**
+ * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_attr, u32 *len_attr)
+{
+ u8 *attr_ptr = NULL;
+ u8 *target_attr_ptr = NULL;
+ u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+ if (len_attr)
+ *len_attr = 0;
+
+ if (!p2p_ie || (p2p_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+ memcmp(p2p_ie + 2, p2p_oui , 4))
+ return attr_ptr;
+
+ /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
+ attr_ptr = p2p_ie + 6; /* goto first attr */
+
+ while (attr_ptr - p2p_ie < p2p_ielen) {
+ /* 3 = 1(Attribute ID) + 2(Length) */
+ u8 attr_id = *attr_ptr;
+ u16 attr_data_len = get_unaligned_le16(attr_ptr + 1);
+ u16 attr_len = attr_data_len + 3;
+
+ if (attr_id == target_attr_id) {
+ target_attr_ptr = attr_ptr;
+
+ if (buf_attr)
+ memcpy(buf_attr, attr_ptr, attr_len);
+ if (len_attr)
+ *len_attr = attr_len;
+ break;
+ } else {
+ attr_ptr += attr_len; /* goto next */
+ }
+ }
+ return target_attr_ptr;
+}
+
+/**
+ * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
+ *
+ * Returns: the address of the specific P2P attribute content found, or NULL
+ */
+u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_content, uint *len_content)
+{
+ u8 *attr_ptr;
+ u32 attr_len;
+
+ if (len_content)
+ *len_content = 0;
+
+ attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
+
+ if (attr_ptr && attr_len) {
+ if (buf_content)
+ memcpy(buf_content, attr_ptr+3, attr_len-3);
+
+ if (len_content)
+ *len_content = attr_len-3;
+
+ return attr_ptr+3;
+ }
+
+ return NULL;
+}
+
+u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
+{
+ u32 a_len;
+
+ *pbuf = attr_id;
+
+ /* u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
+ RTW_PUT_LE16(pbuf + 1, attr_len);
+
+ if (pdata_attr)
+ memcpy(pbuf + 3, pdata_attr, attr_len);
+
+ a_len = attr_len + 3;
+
+ return a_len;
+}
+
+static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
+{
+ u8 *target_attr;
+ u32 target_attr_len;
+ uint ielen = ielen_ori;
+
+ while (1) {
+ target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
+ if (target_attr && target_attr_len) {
+ u8 *next_attr = target_attr+target_attr_len;
+ uint remain_len = ielen-(next_attr-ie);
+
+ memset(target_attr, 0, target_attr_len);
+ memcpy(target_attr, next_attr, remain_len);
+ memset(target_attr+remain_len, 0, target_attr_len);
+ *(ie+1) -= target_attr_len;
+ ielen -= target_attr_len;
+ } else {
+ break;
+ }
+ }
+ return ielen;
+}
+
+void rtw_wlan_bssid_ex_remove_p2p_attr(struct wlan_bssid_ex *bss_ex, u8 attr_id)
+{
+ u8 *p2p_ie;
+ uint p2p_ielen, p2p_ielen_ori;
+
+ p2p_ie = rtw_get_p2p_ie(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_, NULL, &p2p_ielen_ori);
+ if (p2p_ie) {
+ p2p_ielen = rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
+ if (p2p_ielen != p2p_ielen_ori) {
+ u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
+ u8 *next_ie = p2p_ie+p2p_ielen;
+ uint remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
+
+ memcpy(next_ie, next_ie_ori, remain_len);
+ memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
+ bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
+ }
+ }
+}
+
+#endif /* CONFIG_88EU_P2P */
+
+/* Baron adds to avoid FreeBSD warning */
+int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+ /* Single white space is for Linksys APs */
+ if (essid_len == 1 && essid[0] == ' ')
+ return 1;
+
+ /* Otherwise, if the entire essid is 0, we assume it is hidden */
+ while (essid_len) {
+ essid_len--;
+ if (essid[essid_len] != '\0')
+ return 0;
+ }
+
+ return 1;
+}
+
+int ieee80211_get_hdrlen(u16 fc)
+{
+ int hdrlen = 24;
+
+ switch (WLAN_FC_GET_TYPE(fc)) {
+ case RTW_IEEE80211_FTYPE_DATA:
+ if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
+ hdrlen += 2;
+ if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
+ hdrlen += 6; /* Addr4 */
+ break;
+ case RTW_IEEE80211_FTYPE_CTL:
+ switch (WLAN_FC_GET_STYPE(fc)) {
+ case RTW_IEEE80211_STYPE_CTS:
+ case RTW_IEEE80211_STYPE_ACK:
+ hdrlen = 10;
+ break;
+ default:
+ hdrlen = 16;
+ break;
+ }
+ break;
+ }
+
+ return hdrlen;
+}
+
+static int rtw_get_cipher_info(struct wlan_network *pnetwork)
+{
+ u32 wpa_ielen;
+ unsigned char *pbuf;
+ int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
+ int ret = _FAIL;
+ pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+ if (pbuf && (wpa_ielen > 0)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
+ if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+ pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+ pnetwork->BcnInfo.group_cipher = group_cipher;
+ pnetwork->BcnInfo.is_8021x = is8021x;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
+ __func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
+ ret = _SUCCESS;
+ }
+ } else {
+ pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+ if (pbuf && (wpa_ielen > 0)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE\n"));
+ if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE OK!!!\n"));
+ pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+ pnetwork->BcnInfo.group_cipher = group_cipher;
+ pnetwork->BcnInfo.is_8021x = is8021x;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d,"
+ "pnetwork->group_cipher is %d, is_8021x is %d", __func__, pnetwork->BcnInfo.pairwise_cipher,
+ pnetwork->BcnInfo.group_cipher, pnetwork->BcnInfo.is_8021x));
+ ret = _SUCCESS;
+ }
+ }
+ }
+
+ return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+ unsigned short cap = 0;
+ u8 bencrypt = 0;
+ __le16 le_tmp;
+ u16 wpa_len = 0, rsn_len = 0;
+ struct HT_info_element *pht_info = NULL;
+ struct ieee80211_ht_cap *pht_cap = NULL;
+ unsigned int len;
+ unsigned char *p;
+
+ memcpy(&le_tmp, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
+ cap = le16_to_cpu(le_tmp);
+ if (cap & WLAN_CAPABILITY_PRIVACY) {
+ bencrypt = 1;
+ pnetwork->network.Privacy = 1;
+ } else {
+ pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+ }
+ rtw_get_sec_ie(pnetwork->network.IEs , pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+
+ if (rsn_len > 0) {
+ pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+ } else if (wpa_len > 0) {
+ pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+ } else {
+ if (bencrypt)
+ pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+ }
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+ pnetwork->BcnInfo.encryp_protocol));
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+ pnetwork->BcnInfo.encryp_protocol));
+ rtw_get_cipher_info(pnetwork);
+
+ /* get bwmode and ch_offset */
+ /* parsing HT_CAP_IE */
+ p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+ if (p && len > 0) {
+ pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+ pnetwork->BcnInfo.ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+ } else {
+ pnetwork->BcnInfo.ht_cap_info = 0;
+ }
+ /* parsing HT_INFO_IE */
+ p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+ if (p && len > 0) {
+ pht_info = (struct HT_info_element *)(p + 2);
+ pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
+ } else {
+ pnetwork->BcnInfo.ht_info_infos_0 = 0;
+ }
+}
+
+/* show MCS rate, unit: 100Kbps */
+u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char *MCS_rate)
+{
+ u16 max_rate = 0;
+
+ if (rf_type == RF_1T1R) {
+ if (MCS_rate[0] & BIT(7))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1500 : 1350) : ((short_GI_20) ? 722 : 650);
+ else if (MCS_rate[0] & BIT(6))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1350 : 1215) : ((short_GI_20) ? 650 : 585);
+ else if (MCS_rate[0] & BIT(5))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
+ else if (MCS_rate[0] & BIT(4))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
+ else if (MCS_rate[0] & BIT(3))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
+ else if (MCS_rate[0] & BIT(2))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 450 : 405) : ((short_GI_20) ? 217 : 195);
+ else if (MCS_rate[0] & BIT(1))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
+ else if (MCS_rate[0] & BIT(0))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 150 : 135) : ((short_GI_20) ? 72 : 65);
+ } else {
+ if (MCS_rate[1]) {
+ if (MCS_rate[1] & BIT(7))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 3000 : 2700) : ((short_GI_20) ? 1444 : 1300);
+ else if (MCS_rate[1] & BIT(6))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 2700 : 2430) : ((short_GI_20) ? 1300 : 1170);
+ else if (MCS_rate[1] & BIT(5))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 2400 : 2160) : ((short_GI_20) ? 1156 : 1040);
+ else if (MCS_rate[1] & BIT(4))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1800 : 1620) : ((short_GI_20) ? 867 : 780);
+ else if (MCS_rate[1] & BIT(3))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
+ else if (MCS_rate[1] & BIT(2))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
+ else if (MCS_rate[1] & BIT(1))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
+ else if (MCS_rate[1] & BIT(0))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
+ } else {
+ if (MCS_rate[0] & BIT(7))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1500 : 1350) : ((short_GI_20) ? 722 : 650);
+ else if (MCS_rate[0] & BIT(6))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1350 : 1215) : ((short_GI_20) ? 650 : 585);
+ else if (MCS_rate[0] & BIT(5))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
+ else if (MCS_rate[0] & BIT(4))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
+ else if (MCS_rate[0] & BIT(3))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
+ else if (MCS_rate[0] & BIT(2))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 450 : 405) : ((short_GI_20) ? 217 : 195);
+ else if (MCS_rate[0] & BIT(1))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
+ else if (MCS_rate[0] & BIT(0))
+ max_rate = (bw_40MHz) ? ((short_GI_40) ? 150 : 135) : ((short_GI_20) ? 72 : 65);
+ }
+ }
+ return max_rate;
+}
+
+int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
+{
+ const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+ u16 fc;
+ u8 c, a = 0;
+
+ fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
+
+ if ((fc & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE)) !=
+ (RTW_IEEE80211_FTYPE_MGMT|RTW_IEEE80211_STYPE_ACTION))
+ return false;
+
+ c = frame_body[0];
+
+ switch (c) {
+ case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
+ break;
+ default:
+ a = frame_body[1];
+ }
+
+ if (category)
+ *category = c;
+ if (action)
+ *action = a;
+
+ return true;
+}
+
+static const char *_action_public_str[] = {
+ "ACT_PUB_BSSCOEXIST",
+ "ACT_PUB_DSE_ENABLE",
+ "ACT_PUB_DSE_DEENABLE",
+ "ACT_PUB_DSE_REG_LOCATION",
+ "ACT_PUB_EXT_CHL_SWITCH",
+ "ACT_PUB_DSE_MSR_REQ",
+ "ACT_PUB_DSE_MSR_RPRT",
+ "ACT_PUB_MP",
+ "ACT_PUB_DSE_PWR_CONSTRAINT",
+ "ACT_PUB_VENDOR",
+ "ACT_PUB_GAS_INITIAL_REQ",
+ "ACT_PUB_GAS_INITIAL_RSP",
+ "ACT_PUB_GAS_COMEBACK_REQ",
+ "ACT_PUB_GAS_COMEBACK_RSP",
+ "ACT_PUB_TDLS_DISCOVERY_RSP",
+ "ACT_PUB_LOCATION_TRACK",
+ "ACT_PUB_RSVD",
+};
+
+const char *action_public_str(u8 action)
+{
+ action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
+ return _action_public_str[action];
+}
diff --git a/drivers/staging/r8188eu/core/rtw_io.c b/drivers/staging/r8188eu/core/rtw_io.c
new file mode 100644
index 000000000000..db593503e26e
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_io.c
@@ -0,0 +1,323 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+/*
+
+The purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+Compiler Flag Option:
+
+USB:
+ a. USE_ASYNC_IRP: Both sync/async operations are provided.
+
+Only sync read/rtw_write_mem operations are provided.
+
+jackson@realtek.com.tw
+
+*/
+
+#define _RTW_IO_C_
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_io.h>
+#include <osdep_intf.h>
+#include <usb_ops.h>
+
+#define rtw_le16_to_cpu(val) le16_to_cpu(val)
+#define rtw_le32_to_cpu(val) le32_to_cpu(val)
+#define rtw_cpu_to_le16(val) cpu_to_le16(val)
+#define rtw_cpu_to_le32(val) cpu_to_le32(val)
+
+u8 _rtw_read8(struct adapter *adapter, u32 addr)
+{
+ u8 r_val;
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+
+ _read8 = pintfhdl->io_ops._read8;
+ r_val = _read8(pintfhdl, addr);
+
+ return r_val;
+}
+
+u16 _rtw_read16(struct adapter *adapter, u32 addr)
+{
+ u16 r_val;
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+
+ _read16 = pintfhdl->io_ops._read16;
+
+ r_val = _read16(pintfhdl, addr);
+
+ return r_val;
+}
+
+u32 _rtw_read32(struct adapter *adapter, u32 addr)
+{
+ u32 r_val;
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+
+ _read32 = pintfhdl->io_ops._read32;
+
+ r_val = _read32(pintfhdl, addr);
+
+ return r_val;
+}
+
+int _rtw_write8(struct adapter *adapter, u32 addr, u8 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+ int ret;
+
+ _write8 = pintfhdl->io_ops._write8;
+
+ ret = _write8(pintfhdl, addr, val);
+
+
+ return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_write16(struct adapter *adapter, u32 addr, u16 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+ int ret;
+
+ _write16 = pintfhdl->io_ops._write16;
+
+ ret = _write16(pintfhdl, addr, val);
+
+
+ return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32(struct adapter *adapter, u32 addr, u32 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+ int ret;
+
+ _write32 = pintfhdl->io_ops._write32;
+
+ ret = _write32(pintfhdl, addr, val);
+
+
+ return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_writeN(struct adapter *adapter, u32 addr , u32 length , u8 *pdata)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = (struct intf_hdl *)(&(pio_priv->intf));
+ int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
+ int ret;
+
+ _writeN = pintfhdl->io_ops._writeN;
+
+ ret = _writeN(pintfhdl, addr, length, pdata);
+
+
+ return RTW_STATUS_CODE(ret);
+}
+int _rtw_write8_async(struct adapter *adapter, u32 addr, u8 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+ int ret;
+
+ _write8_async = pintfhdl->io_ops._write8_async;
+
+ ret = _write8_async(pintfhdl, addr, val);
+
+
+ return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_write16_async(struct adapter *adapter, u32 addr, u16 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+ int ret;
+
+ _write16_async = pintfhdl->io_ops._write16_async;
+ ret = _write16_async(pintfhdl, addr, val);
+
+ return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_write32_async(struct adapter *adapter, u32 addr, u32 val)
+{
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+ int ret;
+
+ _write32_async = pintfhdl->io_ops._write32_async;
+ ret = _write32_async(pintfhdl, addr, val);
+
+ return RTW_STATUS_CODE(ret);
+}
+
+void _rtw_read_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+ void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+
+ if (adapter->bDriverStopped || adapter->bSurpriseRemoved) {
+ RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
+ ("rtw_read_mem:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
+ adapter->bDriverStopped, adapter->bSurpriseRemoved));
+ return;
+ }
+ _read_mem = pintfhdl->io_ops._read_mem;
+ _read_mem(pintfhdl, addr, cnt, pmem);
+
+}
+
+void _rtw_write_mem(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+ void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+
+
+ _write_mem = pintfhdl->io_ops._write_mem;
+
+ _write_mem(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+ u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+
+
+ if (adapter->bDriverStopped || adapter->bSurpriseRemoved) {
+ RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
+ ("rtw_read_port:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
+ adapter->bDriverStopped, adapter->bSurpriseRemoved));
+ return;
+ }
+
+ _read_port = pintfhdl->io_ops._read_port;
+
+ _read_port(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port_cancel(struct adapter *adapter)
+{
+ void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+ _read_port_cancel = pintfhdl->io_ops._read_port_cancel;
+
+ if (_read_port_cancel)
+ _read_port_cancel(pintfhdl);
+}
+
+u32 _rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+ u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+ u32 ret = _SUCCESS;
+
+
+
+ _write_port = pintfhdl->io_ops._write_port;
+
+ ret = _write_port(pintfhdl, addr, cnt, pmem);
+
+
+
+ return ret;
+}
+
+u32 _rtw_write_port_and_wait(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem, int timeout_ms)
+{
+ int ret = _SUCCESS;
+ struct xmit_buf *pxmitbuf = (struct xmit_buf *)pmem;
+ struct submit_ctx sctx;
+
+ rtw_sctx_init(&sctx, timeout_ms);
+ pxmitbuf->sctx = &sctx;
+
+ ret = _rtw_write_port(adapter, addr, cnt, pmem);
+
+ if (ret == _SUCCESS)
+ ret = rtw_sctx_wait(&sctx);
+
+ return ret;
+}
+
+void _rtw_write_port_cancel(struct adapter *adapter)
+{
+ void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
+ struct io_priv *pio_priv = &adapter->iopriv;
+ struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+ _write_port_cancel = pintfhdl->io_ops._write_port_cancel;
+
+ if (_write_port_cancel)
+ _write_port_cancel(pintfhdl);
+}
+
+int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct _io_ops *pops))
+{
+ struct io_priv *piopriv = &padapter->iopriv;
+ struct intf_hdl *pintf = &piopriv->intf;
+
+ if (set_intf_ops == NULL)
+ return _FAIL;
+
+ piopriv->padapter = padapter;
+ pintf->padapter = padapter;
+ pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+ set_intf_ops(&pintf->io_ops);
+
+ return _SUCCESS;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_ioctl_set.c b/drivers/staging/r8188eu/core/rtw_ioctl_set.c
new file mode 100644
index 000000000000..78974325d8bd
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_ioctl_set.c
@@ -0,0 +1,1118 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_SET_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_ioctl_set.h>
+#include <hal_intf.h>
+
+#include <usb_osintf.h>
+#include <usb_ops.h>
+
+extern void indicate_wx_scan_complete_event(struct adapter *padapter);
+
+#define IS_MAC_ADDRESS_BROADCAST(addr) \
+(\
+ ((addr[0] == 0xff) && (addr[1] == 0xff) && \
+ (addr[2] == 0xff) && (addr[3] == 0xff) && \
+ (addr[4] == 0xff) && (addr[5] == 0xff)) ? true : false \
+)
+
+u8 rtw_validate_ssid(struct ndis_802_11_ssid *ssid)
+{
+ u8 i;
+ u8 ret = true;
+
+ if (ssid->SsidLength > 32) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid length >32\n"));
+ ret = false;
+ goto exit;
+ }
+
+ for (i = 0; i < ssid->SsidLength; i++) {
+ /* wifi, printable ascii code must be supported */
+ if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e))) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid has nonprintabl ascii\n"));
+ ret = false;
+ break;
+ }
+ }
+
+exit:
+
+ return ret;
+}
+
+u8 rtw_do_join(struct adapter *padapter)
+{
+ struct list_head *plist, *phead;
+ u8 *pibss = NULL;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct __queue *queue = &(pmlmepriv->scanned_queue);
+ u8 ret = _SUCCESS;
+
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ phead = get_list_head(queue);
+ plist = phead->next;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("\n rtw_do_join: phead = %p; plist = %p\n\n\n", phead, plist));
+
+ pmlmepriv->cur_network.join_res = -2;
+
+ set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+ pmlmepriv->pscanned = plist;
+
+ pmlmepriv->to_join = true;
+
+ if (list_empty(&queue->queue)) {
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+ /* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */
+ /* we try to issue sitesurvey firstly */
+
+ if (!pmlmepriv->LinkDetectInfo.bBusyTraffic ||
+ pmlmepriv->to_roaming > 0) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_do_join(): site survey if scanned_queue is empty\n."));
+ /* submit site_survey_cmd */
+ ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+ if (_SUCCESS != ret) {
+ pmlmepriv->to_join = false;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_do_join(): site survey return error\n."));
+ }
+ } else {
+ pmlmepriv->to_join = false;
+ ret = _FAIL;
+ }
+
+ goto exit;
+ } else {
+ int select_ret;
+
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+ if (select_ret == _SUCCESS) {
+ pmlmepriv->to_join = false;
+ _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+ } else {
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+ /* submit createbss_cmd to change to a ADHOC_MASTER */
+
+ /* pmlmepriv->lock has been acquired by caller... */
+ struct wlan_bssid_ex *pdev_network = &(padapter->registrypriv.dev_network);
+
+ pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+ pibss = padapter->registrypriv.dev_network.MacAddress;
+
+ memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+ memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+ rtw_update_registrypriv_dev_network(padapter);
+
+ rtw_generate_random_ibss(pibss);
+
+ if (rtw_createbss_cmd(padapter) != _SUCCESS) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>do_goin: rtw_createbss_cmd status FAIL***\n "));
+ ret = false;
+ goto exit;
+ }
+ pmlmepriv->to_join = false;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("***Error => rtw_select_and_join_from_scanned_queue FAIL under STA_Mode***\n "));
+ } else {
+ /* can't associate ; reset under-linking */
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+ /* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */
+ /* we try to issue sitesurvey firstly */
+ if (!pmlmepriv->LinkDetectInfo.bBusyTraffic ||
+ pmlmepriv->to_roaming > 0) {
+ ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+ if (_SUCCESS != ret) {
+ pmlmepriv->to_join = false;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("do_join(): site survey return error\n."));
+ }
+ } else {
+ ret = _FAIL;
+ pmlmepriv->to_join = false;
+ }
+ }
+ }
+ }
+
+exit:
+
+ return ret;
+}
+
+u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid)
+{
+ u8 status = _SUCCESS;
+ u32 cur_time = 0;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ DBG_88E_LEVEL(_drv_info_, "set bssid:%pM\n", bssid);
+
+ if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 &&
+ bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) ||
+ (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF &&
+ bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) {
+ status = _FAIL;
+ goto exit;
+ }
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ DBG_88E("Set BSSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+ goto handle_tkip_countermeasure;
+ else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+ goto release_mlme_lock;
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+ if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN)) {
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)
+ goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+ } else {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set BSSID not the same bssid\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid =%pM\n", (bssid)));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("cur_bssid =%pM\n", (pmlmepriv->cur_network.network.MacAddress)));
+
+ rtw_disassoc_cmd(padapter, 0, true);
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+ rtw_indicate_disconnect(padapter);
+
+ rtw_free_assoc_resources(padapter, 1);
+
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+ _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+ set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+ }
+ }
+ }
+
+handle_tkip_countermeasure:
+ /* should we add something here...? */
+
+ if (padapter->securitypriv.btkip_countermeasure) {
+ cur_time = jiffies;
+
+ if ((cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ) {
+ padapter->securitypriv.btkip_countermeasure = false;
+ padapter->securitypriv.btkip_countermeasure_time = 0;
+ } else {
+ status = _FAIL;
+ goto release_mlme_lock;
+ }
+ }
+
+ memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+ pmlmepriv->assoc_by_bssid = true;
+
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+ pmlmepriv->to_join = true;
+ else
+ status = rtw_do_join(padapter);
+
+release_mlme_lock:
+ spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("rtw_set_802_11_bssid: status=%d\n", status));
+
+ return status;
+}
+
+u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid)
+{
+ u8 status = _SUCCESS;
+ u32 cur_time = 0;
+
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+ DBG_88E_LEVEL(_drv_info_, "set ssid [%s] fw_state=0x%08x\n",
+ ssid->Ssid, get_fwstate(pmlmepriv));
+
+ if (!padapter->hw_init_completed) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("set_ssid: hw_init_completed == false =>exit!!!\n"));
+ status = _FAIL;
+ goto exit;
+ }
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ DBG_88E("Set SSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+ goto handle_tkip_countermeasure;
+ } else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+ goto release_mlme_lock;
+ }
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+ if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+ (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength))) {
+ if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("Set SSID is the same ssid, fw_state = 0x%08x\n",
+ get_fwstate(pmlmepriv)));
+
+ if (!rtw_is_same_ibss(padapter, pnetwork)) {
+ /* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
+ rtw_disassoc_cmd(padapter, 0, true);
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+ rtw_indicate_disconnect(padapter);
+
+ rtw_free_assoc_resources(padapter, 1);
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+ _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+ set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+ }
+ } else {
+ goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+ }
+ } else {
+ rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set SSID not the same ssid\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_ssid =[%s] len = 0x%x\n", ssid->Ssid, (unsigned int)ssid->SsidLength));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("assoc_ssid =[%s] len = 0x%x\n", pmlmepriv->assoc_ssid.Ssid, (unsigned int)pmlmepriv->assoc_ssid.SsidLength));
+
+ rtw_disassoc_cmd(padapter, 0, true);
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+ rtw_indicate_disconnect(padapter);
+
+ rtw_free_assoc_resources(padapter, 1);
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+ _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+ set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+ }
+ }
+ }
+
+handle_tkip_countermeasure:
+
+ if (padapter->securitypriv.btkip_countermeasure) {
+ cur_time = jiffies;
+
+ if ((cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ) {
+ padapter->securitypriv.btkip_countermeasure = false;
+ padapter->securitypriv.btkip_countermeasure_time = 0;
+ } else {
+ status = _FAIL;
+ goto release_mlme_lock;
+ }
+ }
+
+ memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid));
+ pmlmepriv->assoc_by_bssid = false;
+
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+ pmlmepriv->to_join = true;
+ } else {
+ status = rtw_do_join(padapter);
+ }
+
+release_mlme_lock:
+ spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("-rtw_set_802_11_ssid: status =%d\n", status));
+
+ return status;
+}
+
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter,
+ enum ndis_802_11_network_infra networktype)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_network *cur_network = &pmlmepriv->cur_network;
+ enum ndis_802_11_network_infra *pold_state = &(cur_network->network.InfrastructureMode);
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
+ ("+rtw_set_802_11_infrastructure_mode: old =%d new =%d fw_state = 0x%08x\n",
+ *pold_state, networktype, get_fwstate(pmlmepriv)));
+
+ if (*pold_state != networktype) {
+ spin_lock_bh(&pmlmepriv->lock);
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!"));
+ /* DBG_88E("change mode, old_mode =%d, new_mode =%d, fw_state = 0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
+
+ if (*pold_state == Ndis802_11APMode) {
+ /* change to other mode from Ndis802_11APMode */
+ cur_network->join_res = -1;
+
+#ifdef CONFIG_88EU_AP_MODE
+ stop_ap_mode(padapter);
+#endif
+ }
+
+ if ((check_fwstate(pmlmepriv, _FW_LINKED)) ||
+ (*pold_state == Ndis802_11IBSS))
+ rtw_disassoc_cmd(padapter, 0, true);
+
+ if ((check_fwstate(pmlmepriv, _FW_LINKED)) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+ rtw_free_assoc_resources(padapter, 1);
+
+ if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) {
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+ rtw_indicate_disconnect(padapter); /* will clr Linked_state; before this function, we must have chked whether issue dis-assoc_cmd or not */
+ }
+
+ *pold_state = networktype;
+
+ _clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+ switch (networktype) {
+ case Ndis802_11IBSS:
+ set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+ break;
+ case Ndis802_11Infrastructure:
+ set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+ break;
+ case Ndis802_11APMode:
+ set_fwstate(pmlmepriv, WIFI_AP_STATE);
+#ifdef CONFIG_88EU_AP_MODE
+ start_ap_mode(padapter);
+#endif
+ break;
+ case Ndis802_11AutoUnknown:
+ case Ndis802_11InfrastructureMax:
+ break;
+ }
+ spin_unlock_bh(&pmlmepriv->lock);
+ }
+
+ return true;
+}
+
+u8 rtw_set_802_11_disassociate(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("MgntActrtw_set_802_11_disassociate: rtw_indicate_disconnect\n"));
+
+ rtw_disassoc_cmd(padapter, 0, true);
+ rtw_indicate_disconnect(padapter);
+ rtw_free_assoc_resources(padapter, 1);
+ rtw_pwr_wakeup(padapter);
+ }
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ return true;
+}
+
+u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ u8 res = true;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("+rtw_set_802_11_bssid_list_scan(), fw_state =%x\n", get_fwstate(pmlmepriv)));
+
+ if (padapter == NULL) {
+ res = false;
+ goto exit;
+ }
+ if (!padapter->hw_init_completed) {
+ res = false;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n === rtw_set_802_11_bssid_list_scan:hw_init_completed == false ===\n"));
+ goto exit;
+ }
+
+ if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING)) ||
+ (pmlmepriv->LinkDetectInfo.bBusyTraffic)) {
+ /* Scan or linking is in progress, do nothing. */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_set_802_11_bssid_list_scan fail since fw_state = %x\n", get_fwstate(pmlmepriv)));
+ res = true;
+
+ if (check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING)) == true) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n\n"));
+ } else {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###pmlmepriv->sitesurveyctrl.traffic_busy == true\n\n"));
+ }
+ } else {
+ if (rtw_is_scan_deny(padapter)) {
+ DBG_88E(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter));
+ indicate_wx_scan_complete_event(padapter);
+ return _SUCCESS;
+ }
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, NULL, 0);
+
+ spin_unlock_bh(&pmlmepriv->lock);
+ }
+exit:
+
+ return res;
+}
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *padapter, enum ndis_802_11_auth_mode authmode)
+{
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ int res;
+ u8 ret;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_802_11_auth.mode(): mode =%x\n", authmode));
+
+ psecuritypriv->ndisauthtype = authmode;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_authentication_mode:psecuritypriv->ndisauthtype=%d",
+ psecuritypriv->ndisauthtype));
+
+ if (psecuritypriv->ndisauthtype > 3)
+ psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+ res = rtw_set_auth(padapter, psecuritypriv);
+
+ if (res == _SUCCESS)
+ ret = true;
+ else
+ ret = false;
+
+ return ret;
+}
+
+u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct ndis_802_11_wep *wep)
+{
+ int keyid, res;
+ struct security_priv *psecuritypriv = &(padapter->securitypriv);
+ u8 ret = _SUCCESS;
+
+ keyid = wep->KeyIndex & 0x3fffffff;
+
+ if (keyid >= 4) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("MgntActrtw_set_802_11_add_wep:keyid>4 =>fail\n"));
+ ret = false;
+ goto exit;
+ }
+
+ switch (wep->KeyLength) {
+ case 5:
+ psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength = 5\n"));
+ break;
+ case 13:
+ psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength = 13\n"));
+ break;
+ default:
+ psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength!= 5 or 13\n"));
+ break;
+ }
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_add_wep:befor memcpy, wep->KeyLength = 0x%x wep->KeyIndex = 0x%x keyid =%x\n",
+ wep->KeyLength, wep->KeyIndex, keyid));
+
+ memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength);
+
+ psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
+
+ psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_add_wep:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x\n",
+ psecuritypriv->dot11DefKey[keyid].skey[0],
+ psecuritypriv->dot11DefKey[keyid].skey[1],
+ psecuritypriv->dot11DefKey[keyid].skey[2],
+ psecuritypriv->dot11DefKey[keyid].skey[3],
+ psecuritypriv->dot11DefKey[keyid].skey[4],
+ psecuritypriv->dot11DefKey[keyid].skey[5],
+ psecuritypriv->dot11DefKey[keyid].skey[6],
+ psecuritypriv->dot11DefKey[keyid].skey[7],
+ psecuritypriv->dot11DefKey[keyid].skey[8],
+ psecuritypriv->dot11DefKey[keyid].skey[9],
+ psecuritypriv->dot11DefKey[keyid].skey[10],
+ psecuritypriv->dot11DefKey[keyid].skey[11],
+ psecuritypriv->dot11DefKey[keyid].skey[12]));
+
+ res = rtw_set_key(padapter, psecuritypriv, keyid, 1);
+
+ if (res == _FAIL)
+ ret = false;
+exit:
+
+ return ret;
+}
+
+u8 rtw_set_802_11_remove_wep(struct adapter *padapter, u32 keyindex)
+{
+ u8 ret = _SUCCESS;
+
+ if (keyindex >= 0x80000000 || padapter == NULL) {
+ ret = false;
+ goto exit;
+ } else {
+ int res;
+ struct security_priv *psecuritypriv = &(padapter->securitypriv);
+ if (keyindex < 4) {
+ memset(&psecuritypriv->dot11DefKey[keyindex], 0, 16);
+ res = rtw_set_key(padapter, psecuritypriv, keyindex, 0);
+ psecuritypriv->dot11DefKeylen[keyindex] = 0;
+ if (res == _FAIL)
+ ret = _FAIL;
+ } else {
+ ret = _FAIL;
+ }
+ }
+exit:
+
+ return ret;
+}
+
+u8 rtw_set_802_11_add_key(struct adapter *padapter, struct ndis_802_11_key *key)
+{
+ uint encryptionalgo;
+ u8 *pbssid;
+ struct sta_info *stainfo;
+ u8 bgroup = false;
+ u8 bgrouptkey = false;/* can be removed later */
+ u8 ret = _SUCCESS;
+
+ if (((key->KeyIndex & 0x80000000) == 0) && ((key->KeyIndex & 0x40000000) > 0)) {
+ /* It is invalid to clear bit 31 and set bit 30. If the miniport driver encounters this combination, */
+ /* it must fail the request and return NDIS_STATUS_INVALID_DATA. */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_add_key: ((key->KeyIndex & 0x80000000)==0)[=%d]",
+ (int)(key->KeyIndex & 0x80000000) == 0));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_add_key:((key->KeyIndex & 0x40000000)>0)[=%d]",
+ (int)(key->KeyIndex & 0x40000000) > 0));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+ ("rtw_set_802_11_add_key: key->KeyIndex=%d\n",
+ (int)key->KeyIndex));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if (key->KeyIndex & 0x40000000) {
+ /* Pairwise key */
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY: +++++ Pairwise key +++++\n"));
+
+ pbssid = get_bssid(&padapter->mlmepriv);
+ stainfo = rtw_get_stainfo(&padapter->stapriv, pbssid);
+
+ if ((stainfo != NULL) && (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("OID_802_11_ADD_KEY:(stainfo!=NULL)&&(Adapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)\n"));
+ encryptionalgo = stainfo->dot118021XPrivacy;
+ } else {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY: stainfo == NULL)||(Adapter->securitypriv.dot11AuthAlgrthm!= dot11AuthAlgrthm_8021X)\n"));
+ encryptionalgo = padapter->securitypriv.dot11PrivacyAlgrthm;
+ }
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("rtw_set_802_11_add_key: (encryptionalgo==%d)!\n",
+ encryptionalgo));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("rtw_set_802_11_add_key: (Adapter->securitypriv.dot11PrivacyAlgrthm==%d)!\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("rtw_set_802_11_add_key: (Adapter->securitypriv.dot11AuthAlgrthm==%d)!\n",
+ padapter->securitypriv.dot11AuthAlgrthm));
+
+ if ((stainfo != NULL))
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("rtw_set_802_11_add_key: (stainfo->dot118021XPrivacy==%d)!\n",
+ stainfo->dot118021XPrivacy));
+
+ if (key->KeyIndex & 0x000000FF) {
+ /* The key index is specified in the lower 8 bits by values of zero to 255. */
+ /* The key index should be set to zero for a Pairwise key, and the driver should fail with */
+ /* NDIS_STATUS_INVALID_DATA if the lower 8 bits is not zero */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, (" key->KeyIndex & 0x000000FF.\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* check BSSID */
+ if (IS_MAC_ADDRESS_BROADCAST(key->BSSID) == true) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("MacAddr_isBcst(key->BSSID)\n"));
+ ret = false;
+ goto exit;
+ }
+
+ /* Check key length for TKIP. */
+ if ((encryptionalgo == _TKIP_) && (key->KeyLength != 32)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("TKIP KeyLength:0x%x != 32\n", key->KeyLength));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* Check key length for AES. */
+ if ((encryptionalgo == _AES_) && (key->KeyLength != 16)) {
+ /* For our supplicant, EAPPkt9x.vxd, cannot differentiate TKIP and AES case. */
+ if (key->KeyLength == 32) {
+ key->KeyLength = 16;
+ } else {
+ ret = _FAIL;
+ goto exit;
+ }
+ }
+
+ /* Check key length for WEP. For NDTEST, 2005.01.27, by rcnjko. */
+ if ((encryptionalgo == _WEP40_ || encryptionalgo == _WEP104_) &&
+ (key->KeyLength != 5 && key->KeyLength != 13)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("WEP KeyLength:0x%x != 5 or 13\n", key->KeyLength));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ bgroup = false;
+
+ /* Check the pairwise key. Added by Annie, 2005-07-06. */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("[Pairwise Key set]\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("key index: 0x%8x(0x%8x)\n", key->KeyIndex, (key->KeyIndex&0x3)));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("key Length: %d\n", key->KeyLength));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n"));
+
+ } else {
+ /* Group key - KeyIndex(BIT30 == 0) */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY: +++++ Group key +++++\n"));
+
+ /* when add wep key through add key and didn't assigned encryption type before */
+ if ((padapter->securitypriv.ndisauthtype <= 3) &&
+ (padapter->securitypriv.dot118021XGrpPrivacy == 0)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("keylen =%d(Adapter->securitypriv.dot11PrivacyAlgrthm=%x )padapter->securitypriv.dot118021XGrpPrivacy(%x)\n",
+ key->KeyLength, padapter->securitypriv.dot11PrivacyAlgrthm,
+ padapter->securitypriv.dot118021XGrpPrivacy));
+ switch (key->KeyLength) {
+ case 5:
+ padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("Adapter->securitypriv.dot11PrivacyAlgrthm=%x key->KeyLength=%u\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm, key->KeyLength));
+ break;
+ case 13:
+ padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("Adapter->securitypriv.dot11PrivacyAlgrthm=%x key->KeyLength=%u\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm, key->KeyLength));
+ break;
+ default:
+ padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("Adapter->securitypriv.dot11PrivacyAlgrthm=%x key->KeyLength=%u\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm, key->KeyLength));
+ break;
+ }
+
+ encryptionalgo = padapter->securitypriv.dot11PrivacyAlgrthm;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ (" Adapter->securitypriv.dot11PrivacyAlgrthm=%x\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm));
+
+ } else {
+ encryptionalgo = padapter->securitypriv.dot118021XGrpPrivacy;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("(Adapter->securitypriv.dot11PrivacyAlgrthm=%x)encryptionalgo(%x)=padapter->securitypriv.dot118021XGrpPrivacy(%x)keylen=%d\n",
+ padapter->securitypriv.dot11PrivacyAlgrthm, encryptionalgo,
+ padapter->securitypriv.dot118021XGrpPrivacy, key->KeyLength));
+ }
+
+ if ((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE) == true) && (IS_MAC_ADDRESS_BROADCAST(key->BSSID) == false)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ (" IBSS but BSSID is not Broadcast Address.\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* Check key length for TKIP */
+ if ((encryptionalgo == _TKIP_) && (key->KeyLength != 32)) {
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ (" TKIP GTK KeyLength:%u != 32\n", key->KeyLength));
+ ret = _FAIL;
+ goto exit;
+ } else if (encryptionalgo == _AES_ && (key->KeyLength != 16 && key->KeyLength != 32)) {
+ /* Check key length for AES */
+ /* For NDTEST, we allow keylen = 32 in this case. 2005.01.27, by rcnjko. */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("<=== SetInfo, OID_802_11_ADD_KEY: AES GTK KeyLength:%u != 16 or 32\n",
+ key->KeyLength));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* Change the key length for EAPPkt9x.vxd. Added by Annie, 2005-11-03. */
+ if ((encryptionalgo == _AES_) && (key->KeyLength == 32)) {
+ key->KeyLength = 16;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("AES key length changed: %u\n", key->KeyLength));
+ }
+
+ if (key->KeyIndex & 0x8000000) {/* error ??? 0x8000_0000 */
+ bgrouptkey = true;
+ }
+
+ if ((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE)) &&
+ (check_fwstate(&padapter->mlmepriv, _FW_LINKED)))
+ bgrouptkey = true;
+ bgroup = true;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("[Group Key set]\n"));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n")) ;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("key index: 0x%8x(0x%8x)\n", key->KeyIndex, (key->KeyIndex&0x3)));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("key Length: %d\n", key->KeyLength)) ;
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("------------------------------------------\n"));
+ }
+
+ /* If WEP encryption algorithm, just call rtw_set_802_11_add_wep(). */
+ if ((padapter->securitypriv.dot11AuthAlgrthm != dot11AuthAlgrthm_8021X) &&
+ (encryptionalgo == _WEP40_ || encryptionalgo == _WEP104_)) {
+ u32 keyindex;
+ u32 len = FIELD_OFFSET(struct ndis_802_11_key, KeyMaterial) + key->KeyLength;
+ struct ndis_802_11_wep *wep = &padapter->securitypriv.ndiswep;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY: +++++ WEP key +++++\n"));
+
+ wep->Length = len;
+ keyindex = key->KeyIndex&0x7fffffff;
+ wep->KeyIndex = keyindex ;
+ wep->KeyLength = key->KeyLength;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY:Before memcpy\n"));
+
+ memcpy(wep->KeyMaterial, key->KeyMaterial, key->KeyLength);
+ memcpy(&(padapter->securitypriv.dot11DefKey[keyindex].skey[0]), key->KeyMaterial, key->KeyLength);
+
+ padapter->securitypriv.dot11DefKeylen[keyindex] = key->KeyLength;
+ padapter->securitypriv.dot11PrivacyKeyIndex = keyindex;
+
+ ret = rtw_set_802_11_add_wep(padapter, wep);
+ goto exit;
+ }
+ if (key->KeyIndex & 0x20000000) {
+ /* SetRSC */
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("OID_802_11_ADD_KEY: +++++ SetRSC+++++\n"));
+ if (bgroup) {
+ unsigned long long keysrc = key->KeyRSC & 0x00FFFFFFFFFFFFULL;
+ memcpy(&padapter->securitypriv.dot11Grprxpn, &keysrc, 8);
+ } else {
+ unsigned long long keysrc = key->KeyRSC & 0x00FFFFFFFFFFFFULL;
+ memcpy(&padapter->securitypriv.dot11Grptxpn, &keysrc, 8);
+ }
+ }
+
+ /* Indicate this key idx is used for TX */
+ /* Save the key in KeyMaterial */
+ if (bgroup) { /* Group transmit key */
+ int res;
+
+ if (bgrouptkey)
+ padapter->securitypriv.dot118021XGrpKeyid = (u8)key->KeyIndex;
+ if ((key->KeyIndex&0x3) == 0) {
+ ret = _FAIL;
+ goto exit;
+ }
+ memset(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+ memset(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+ memset(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+
+ if ((key->KeyIndex & 0x10000000)) {
+ memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+ memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("\n rtw_set_802_11_add_key:rx mic :0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[0],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[1],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[2],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[3],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[4],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[5],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[6],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[7]));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n rtw_set_802_11_add_key:set Group mic key!!!!!!!!\n"));
+ } else {
+ memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+ memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("\n rtw_set_802_11_add_key:rx mic :0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[0],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[1],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[2],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[3],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[4],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[5],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[6],
+ padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[7]));
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+ ("\n rtw_set_802_11_add_key:set Group mic key!!!!!!!!\n"));
+ }
+
+ /* set group key by index */
+ memcpy(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial, key->KeyLength);
+
+ key->KeyIndex = key->KeyIndex & 0x03;
+
+ padapter->securitypriv.binstallGrpkey = true;
+
+ padapter->securitypriv.bcheck_grpkey = false;
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("reset group key"));
+
+ res = rtw_set_key(padapter, &padapter->securitypriv, key->KeyIndex, 1);
+
+ if (res == _FAIL)
+ ret = _FAIL;
+
+ goto exit;
+
+ } else { /* Pairwise Key */
+ u8 res;
+
+ pbssid = get_bssid(&padapter->mlmepriv);
+ stainfo = rtw_get_stainfo(&padapter->stapriv, pbssid);
+
+ if (stainfo != NULL) {
+ memset(&stainfo->dot118021x_UncstKey, 0, 16);/* clear keybuffer */
+
+ memcpy(&stainfo->dot118021x_UncstKey, key->KeyMaterial, 16);
+
+ if (encryptionalgo == _TKIP_) {
+ padapter->securitypriv.busetkipkey = false;
+
+ /* _set_timer(&padapter->securitypriv.tkip_timer, 50); */
+
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n========== _set_timer\n"));
+
+ /* if TKIP, save the Receive/Transmit MIC key in KeyMaterial[128-255] */
+ if ((key->KeyIndex & 0x10000000)) {
+ memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 16, 8);
+ memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 24, 8);
+
+ } else {
+ memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 24, 8);
+ memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 16, 8);
+ }
+ }
+
+ /* Set key to CAM through H2C command */
+ if (bgrouptkey) { /* never go to here */
+ res = rtw_setstakey_cmd(padapter, (unsigned char *)stainfo, false);
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n rtw_set_802_11_add_key:rtw_setstakey_cmd(group)\n"));
+ } else {
+ res = rtw_setstakey_cmd(padapter, (unsigned char *)stainfo, true);
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n rtw_set_802_11_add_key:rtw_setstakey_cmd(unicast)\n"));
+ }
+ if (!res)
+ ret = _FAIL;
+ }
+ }
+exit:
+
+ return ret;
+}
+
+u8 rtw_set_802_11_remove_key(struct adapter *padapter, struct ndis_802_11_remove_key *key)
+{
+ u8 *pbssid;
+ struct sta_info *stainfo;
+ u8 bgroup = (key->KeyIndex & 0x4000000) > 0 ? false : true;
+ u8 keyIndex = (u8)key->KeyIndex & 0x03;
+ u8 ret = _SUCCESS;
+
+ if ((key->KeyIndex & 0xbffffffc) > 0) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if (bgroup) {
+ /* clear group key by index */
+
+ memset(&padapter->securitypriv.dot118021XGrpKey[keyIndex], 0, 16);
+
+ /* \todo Send a H2C Command to Firmware for removing this Key in CAM Entry. */
+ } else {
+ pbssid = get_bssid(&padapter->mlmepriv);
+ stainfo = rtw_get_stainfo(&padapter->stapriv, pbssid);
+ if (stainfo) {
+ /* clear key by BSSID */
+ memset(&stainfo->dot118021x_UncstKey, 0, 16);
+
+ /* \todo Send a H2C Command to Firmware for disable this Key in CAM Entry. */
+ } else {
+ ret = _FAIL;
+ goto exit;
+ }
+ }
+exit:
+
+ return ret;
+}
+
+/*
+* rtw_get_cur_max_rate -
+* @adapter: pointer to struct adapter structure
+*
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(struct adapter *adapter)
+{
+ int i = 0;
+ u8 *p;
+ u16 rate = 0, max_rate = 0;
+ struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct registry_priv *pregistrypriv = &adapter->registrypriv;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network;
+ struct ieee80211_ht_cap *pht_capie;
+ u8 rf_type = 0;
+ u8 bw_40MHz = 0, short_GI_20 = 0, short_GI_40 = 0;
+ u16 mcs_rate = 0;
+ u32 ht_ielen = 0;
+
+ if (adapter->registrypriv.mp_mode == 1) {
+ if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+ return 0;
+ }
+
+ if ((!check_fwstate(pmlmepriv, _FW_LINKED)) &&
+ (!check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+ return 0;
+
+ if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N|WIRELESS_11_5N)) {
+ p = rtw_get_ie(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pcur_bss->IELength-12);
+ if (p && ht_ielen > 0) {
+ pht_capie = (struct ieee80211_ht_cap *)(p+2);
+
+ memcpy(&mcs_rate , pht_capie->mcs.rx_mask, 2);
+
+ /* cur_bwmod is updated by beacon, pmlmeinfo is updated by association response */
+ bw_40MHz = (pmlmeext->cur_bwmode && (HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH & pmlmeinfo->HT_info.infos[0])) ? 1 : 0;
+
+ short_GI_20 = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & IEEE80211_HT_CAP_SGI_20) ? 1 : 0;
+ short_GI_40 = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & IEEE80211_HT_CAP_SGI_40) ? 1 : 0;
+
+ rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+ max_rate = rtw_mcs_rate(
+ rf_type,
+ bw_40MHz & (pregistrypriv->cbw40_enable),
+ short_GI_20,
+ short_GI_40,
+ pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate
+ );
+ }
+ } else {
+ while ((pcur_bss->SupportedRates[i] != 0) && (pcur_bss->SupportedRates[i] != 0xFF)) {
+ rate = pcur_bss->SupportedRates[i]&0x7F;
+ if (rate > max_rate)
+ max_rate = rate;
+ i++;
+ }
+
+ max_rate = max_rate*10/2;
+ }
+
+ return max_rate;
+}
+
+/*
+* rtw_set_scan_mode -
+* @adapter: pointer to struct adapter structure
+* @scan_mode:
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_scan_mode(struct adapter *adapter, enum rt_scan_type scan_mode)
+{
+ if (scan_mode != SCAN_ACTIVE && scan_mode != SCAN_PASSIVE)
+ return _FAIL;
+
+ adapter->mlmepriv.scan_mode = scan_mode;
+
+ return _SUCCESS;
+}
+
+/*
+* rtw_set_channel_plan -
+* @adapter: pointer to struct adapter structure
+* @channel_plan:
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_channel_plan(struct adapter *adapter, u8 channel_plan)
+{
+ /* handle by cmd_thread to sync with scan operation */
+ return rtw_set_chplan_cmd(adapter, channel_plan, 1);
+}
+
+/*
+* rtw_set_country -
+* @adapter: pointer to struct adapter structure
+* @country_code: string of country code
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_country(struct adapter *adapter, const char *country_code)
+{
+ int channel_plan = RT_CHANNEL_DOMAIN_WORLD_WIDE_5G;
+
+ DBG_88E("%s country_code:%s\n", __func__, country_code);
+
+ /* TODO: should have a table to match country code and RT_CHANNEL_DOMAIN */
+ /* TODO: should consider 2-character and 3-character country code */
+ if (0 == strcmp(country_code, "US"))
+ channel_plan = RT_CHANNEL_DOMAIN_FCC;
+ else if (0 == strcmp(country_code, "EU"))
+ channel_plan = RT_CHANNEL_DOMAIN_ETSI;
+ else if (0 == strcmp(country_code, "JP"))
+ channel_plan = RT_CHANNEL_DOMAIN_MKK;
+ else if (0 == strcmp(country_code, "CN"))
+ channel_plan = RT_CHANNEL_DOMAIN_CHINA;
+ else
+ DBG_88E("%s unknown country_code:%s\n", __func__, country_code);
+
+ return rtw_set_channel_plan(adapter, channel_plan);
+}
diff --git a/drivers/staging/r8188eu/core/rtw_iol.c b/drivers/staging/r8188eu/core/rtw_iol.c
new file mode 100644
index 000000000000..e6fdd32f9a3f
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_iol.c
@@ -0,0 +1,209 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#include<rtw_iol.h>
+
+struct xmit_frame *rtw_IOL_accquire_xmit_frame(struct adapter *adapter)
+{
+ struct xmit_frame *xmit_frame;
+ struct xmit_buf *xmitbuf;
+ struct pkt_attrib *pattrib;
+ struct xmit_priv *pxmitpriv = &(adapter->xmitpriv);
+
+ xmit_frame = rtw_alloc_xmitframe(pxmitpriv);
+ if (xmit_frame == NULL) {
+ DBG_88E("%s rtw_alloc_xmitframe return null\n", __func__);
+ goto exit;
+ }
+
+ xmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+ if (xmitbuf == NULL) {
+ DBG_88E("%s rtw_alloc_xmitbuf return null\n", __func__);
+ rtw_free_xmitframe(pxmitpriv, xmit_frame);
+ xmit_frame = NULL;
+ goto exit;
+ }
+
+ xmit_frame->frame_tag = MGNT_FRAMETAG;
+ xmit_frame->pxmitbuf = xmitbuf;
+ xmit_frame->buf_addr = xmitbuf->pbuf;
+ xmitbuf->priv_data = xmit_frame;
+
+ pattrib = &xmit_frame->attrib;
+ update_mgntframe_attrib(adapter, pattrib);
+ pattrib->qsel = 0x10;/* Beacon */
+ pattrib->subtype = WIFI_BEACON;
+ pattrib->pktlen = 0;
+ pattrib->last_txcmdsz = 0;
+exit:
+ return xmit_frame;
+}
+
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
+{
+ struct pkt_attrib *pattrib = &xmit_frame->attrib;
+ u16 buf_offset;
+ u32 ori_len;
+
+ buf_offset = TXDESC_OFFSET;
+ ori_len = buf_offset+pattrib->pktlen;
+
+ /* check if the io_buf can accommodate new cmds */
+ if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) {
+ DBG_88E("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n",
+ __func__ , ori_len + cmd_len + 8, MAX_XMITBUF_SZ);
+ return _FAIL;
+ }
+
+ memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
+ pattrib->pktlen += cmd_len;
+ pattrib->last_txcmdsz += cmd_len;
+
+ return _SUCCESS;
+}
+
+bool rtw_IOL_applied(struct adapter *adapter)
+{
+ if (1 == adapter->registrypriv.fw_iol)
+ return true;
+
+ if ((2 == adapter->registrypriv.fw_iol) && (!adapter_to_dvobj(adapter)->ishighspeed))
+ return true;
+ return false;
+}
+
+int rtw_IOL_exec_cmds_sync(struct adapter *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
+{
+ return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms, bndy_cnt);
+}
+
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+ return _SUCCESS;
+}
+
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
+{
+ struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0};
+
+ cmd.address = cpu_to_le16(addr);
+ cmd.data = cpu_to_le32(value);
+
+ if (mask != 0xFF) {
+ cmd.length = 12;
+ cmd.mask = cpu_to_le32(mask);
+ }
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask)
+{
+ struct ioreg_cfg cmd = {8, IOREG_CMD_WW_REG, 0x0, 0x0, 0x0};
+
+ cmd.address = cpu_to_le16(addr);
+ cmd.data = cpu_to_le32(value);
+
+ if (mask != 0xFFFF) {
+ cmd.length = 12;
+ cmd.mask = cpu_to_le32(mask);
+ }
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
+{
+ struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0};
+
+ cmd.address = cpu_to_le16(addr);
+ cmd.data = cpu_to_le32(value);
+
+ if (mask != 0xFFFFFFFF) {
+ cmd.length = 12;
+ cmd.mask = cpu_to_le32(mask);
+ }
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
+{
+ struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0};
+
+ cmd.address = cpu_to_le16((rf_path<<8) | ((addr) & 0xFF));
+ cmd.data = cpu_to_le32(value);
+
+ if (mask != 0x000FFFFF) {
+ cmd.length = 12;
+ cmd.mask = cpu_to_le32(mask);
+ }
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+}
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+ struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+ cmd.address = cpu_to_le16(us);
+
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+ struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+
+ cmd.address = cpu_to_le16(ms);
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+ struct ioreg_cfg cmd = {4, IOREG_CMD_END, cpu_to_le16(0xFFFF), cpu_to_le32(0xFF), 0x0};
+
+ return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
+{
+ u8 is_cmd_bndy = false;
+ if (((pxmit_frame->attrib.pktlen+32)%256) + 8 >= 256) {
+ rtw_IOL_append_END_cmd(pxmit_frame);
+ pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen+32)/256)+1)*256);
+
+ pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
+ is_cmd_bndy = true;
+ }
+ return is_cmd_bndy;
+}
+
+void rtw_IOL_cmd_buf_dump(struct adapter *Adapter, int buf_len, u8 *pbuf)
+{
+ int i;
+ int j = 1;
+
+ pr_info("###### %s ######\n", __func__);
+ for (i = 0; i < buf_len; i++) {
+ printk("%02x-", *(pbuf+i));
+
+ if (j%32 == 0)
+ printk("\n");
+ j++;
+ }
+ printk("\n");
+ pr_info("=============ioreg_cmd len=%d===============\n", buf_len);
+}
diff --git a/drivers/staging/r8188eu/core/rtw_led.c b/drivers/staging/r8188eu/core/rtw_led.c
new file mode 100644
index 000000000000..6422dc57853a
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_led.c
@@ -0,0 +1,1705 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include "rtw_led.h"
+
+/* */
+/* Description: */
+/* Callback function of LED BlinkTimer, */
+/* it just schedules to corresponding BlinkWorkItem/led_blink_hdl */
+/* */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+void BlinkTimerCallback(struct timer_list *t)
+#else
+void BlinkTimerCallback(void *data)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct LED_871x *pLed = from_timer(pLed, t, BlinkTimer);
+#else
+ struct LED_871x *pLed = (struct LED_871x *)data;
+#endif
+ struct adapter *padapter = pLed->padapter;
+
+ if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
+ return;
+
+ _set_workitem(&(pLed->BlinkWorkItem));
+}
+
+/* */
+/* Description: */
+/* Callback function of LED BlinkWorkItem. */
+/* We dispatch acture LED blink action according to LedStrategy. */
+/* */
+void BlinkWorkItemCallback(struct work_struct *work)
+{
+ struct LED_871x *pLed = container_of(work, struct LED_871x, BlinkWorkItem);
+ BlinkHandler(pLed);
+}
+
+/* */
+/* Description: */
+/* Reset status of LED_871x object. */
+/* */
+void ResetLedStatus(struct LED_871x *pLed)
+{
+ pLed->CurrLedState = RTW_LED_OFF; /* Current LED state. */
+ pLed->bLedOn = false; /* true if LED is ON, false if LED is OFF. */
+
+ pLed->bLedBlinkInProgress = false; /* true if it is blinking, false o.w.. */
+ pLed->bLedWPSBlinkInProgress = false;
+
+ pLed->BlinkTimes = 0; /* Number of times to toggle led state for blinking. */
+ pLed->BlinkingLedState = LED_UNKNOWN; /* Next state for blinking, either RTW_LED_ON or RTW_LED_OFF are. */
+
+ pLed->bLedNoLinkBlinkInProgress = false;
+ pLed->bLedLinkBlinkInProgress = false;
+ pLed->bLedStartToLinkBlinkInProgress = false;
+ pLed->bLedScanBlinkInProgress = false;
+}
+
+/*Description: */
+/* Initialize an LED_871x object. */
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed, enum LED_PIN_871x LedPin)
+{
+ pLed->padapter = padapter;
+ pLed->LedPin = LedPin;
+
+ ResetLedStatus(pLed);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ timer_setup(&pLed->BlinkTimer, BlinkTimerCallback, 0);
+#else
+ _init_timer(&(pLed->BlinkTimer), padapter->pnetdev, BlinkTimerCallback, pLed);
+#endif
+ _init_workitem(&(pLed->BlinkWorkItem), BlinkWorkItemCallback, pLed);
+}
+
+/* */
+/* Description: */
+/* DeInitialize an LED_871x object. */
+/* */
+void DeInitLed871x(struct LED_871x *pLed)
+{
+ _cancel_workitem_sync(&(pLed->BlinkWorkItem));
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ ResetLedStatus(pLed);
+}
+
+/* */
+/* Description: */
+/* Implementation of LED blinking behavior. */
+/* It toggle off LED and schedule corresponding timer if necessary. */
+/* */
+
+static void SwLedBlink(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ /* Determine if we shall change LED state again. */
+ pLed->BlinkTimes--;
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_NORMAL:
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ break;
+ case LED_BLINK_StartToBlink:
+ if (check_fwstate(pmlmepriv, _FW_LINKED) && check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+ bStopBlinking = true;
+ if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+ bStopBlinking = true;
+ else if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ break;
+ case LED_BLINK_WPS:
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ break;
+ default:
+ bStopBlinking = true;
+ break;
+ }
+
+ if (bStopBlinking) {
+ /* if (padapter->pwrctrlpriv.cpwm >= PS_STATE_S2) */
+ if (0) {
+ SwLedOff(padapter, pLed);
+ } else if ((check_fwstate(pmlmepriv, _FW_LINKED)) && (!pLed->bLedOn)) {
+ SwLedOn(padapter, pLed);
+ } else if ((check_fwstate(pmlmepriv, _FW_LINKED)) && pLed->bLedOn) {
+ SwLedOff(padapter, pLed);
+ }
+ pLed->BlinkTimes = 0;
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ /* Assign LED state to toggle. */
+ if (pLed->BlinkingLedState == RTW_LED_ON)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+
+ /* Schedule a timer to toggle LED state. */
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_NORMAL:
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ break;
+ case LED_BLINK_SLOWLY:
+ case LED_BLINK_StartToBlink:
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ break;
+ case LED_BLINK_WPS:
+ if (pLed->BlinkingLedState == RTW_LED_ON)
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
+ else
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
+ break;
+ default:
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ break;
+ }
+ }
+}
+
+static void SwLedBlink1(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ ResetLedStatus(pLed);
+ return;
+ }
+
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_SLOWLY:
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ break;
+ case LED_BLINK_NORMAL:
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ break;
+ case LED_BLINK_SCAN:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->bLedLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_NORMAL;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedScanBlinkInProgress = false;
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_BLINK_TXRX:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->bLedLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_NORMAL;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->BlinkTimes = 0;
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_BLINK_WPS:
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ break;
+ case LED_BLINK_WPS_STOP: /* WPS success */
+ if (pLed->BlinkingLedState == RTW_LED_ON)
+ bStopBlinking = false;
+ else
+ bStopBlinking = true;
+
+ if (bStopBlinking) {
+ pLed->bLedLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_NORMAL;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+
+ pLed->bLedWPSBlinkInProgress = false;
+ } else {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+static void SwLedBlink2(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_SCAN:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
+
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedScanBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_TXRX:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+static void SwLedBlink3(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ if (pLed->CurrLedState != LED_BLINK_WPS_STOP)
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_SCAN:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (!pLed->bLedOn)
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedOn)
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedScanBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_TXRX:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (!pLed->bLedOn)
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ } else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+
+ if (pLed->bLedOn)
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_WPS:
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ break;
+ case LED_BLINK_WPS_STOP: /* WPS success */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+ bStopBlinking = false;
+ } else {
+ bStopBlinking = true;
+ }
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+static void SwLedBlink4(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct LED_871x *pLed1 = &(ledpriv->SwLed1);
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ if (!pLed1->bLedWPSBlinkInProgress && pLed1->BlinkingLedState == LED_UNKNOWN) {
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ pLed1->CurrLedState = RTW_LED_OFF;
+ SwLedOff(padapter, pLed1);
+ }
+
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_SLOWLY:
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ break;
+ case LED_BLINK_StartToBlink:
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ }
+ break;
+ case LED_BLINK_SCAN:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = false;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->bLedNoLinkBlinkInProgress = false;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ }
+ pLed->bLedScanBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_TXRX:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ }
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_WPS:
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ }
+ break;
+ case LED_BLINK_WPS_STOP: /* WPS authentication fail */
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ break;
+ case LED_BLINK_WPS_STOP_OVERLAP: /* WPS session overlap */
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0) {
+ if (pLed->bLedOn)
+ pLed->BlinkTimes = 1;
+ else
+ bStopBlinking = true;
+ }
+
+ if (bStopBlinking) {
+ pLed->BlinkTimes = 10;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ }
+ break;
+ default:
+ break;
+ }
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState));
+}
+
+static void SwLedBlink5(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ u8 bStopBlinking = false;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ switch (pLed->CurrLedState) {
+ case LED_BLINK_SCAN:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedOn)
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (!pLed->bLedOn)
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+
+ pLed->bLedScanBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ }
+ break;
+ case LED_BLINK_TXRX:
+ pLed->BlinkTimes--;
+ if (pLed->BlinkTimes == 0)
+ bStopBlinking = true;
+
+ if (bStopBlinking) {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedOn)
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (!pLed->bLedOn)
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+
+ pLed->bLedBlinkInProgress = false;
+ } else {
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
+ SwLedOff(padapter, pLed);
+ } else {
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState));
+}
+
+static void SwLedBlink6(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+
+ /* Change LED according to BlinkingLedState specified. */
+ if (pLed->BlinkingLedState == RTW_LED_ON) {
+ SwLedOn(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
+ } else {
+ SwLedOff(padapter, pLed);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("<==== blink6\n"));
+}
+
+ /* ALPHA, added by chiyoko, 20090106 */
+static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct LED_871x *pLed = &(ledpriv->SwLed0);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ switch (LedAction) {
+ case LED_CTL_POWER_ON:
+ case LED_CTL_START_TO_LINK:
+ case LED_CTL_NO_LINK:
+ if (!pLed->bLedNoLinkBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_LINK:
+ if (!pLed->bLedLinkBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_NORMAL;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_SITE_SURVEY:
+ if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+ ;
+ } else if (!pLed->bLedScanBlinkInProgress) {
+ if (IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedScanBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SCAN;
+ pLed->BlinkTimes = 24;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_TX:
+ case LED_CTL_RX:
+ if (!pLed->bLedBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ pLed->bLedBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_TXRX;
+ pLed->BlinkTimes = 2;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_START_WPS: /* wait until xinpin finish */
+ case LED_CTL_START_WPS_BOTTON:
+ if (!pLed->bLedWPSBlinkInProgress) {
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ pLed->bLedWPSBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_WPS;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_STOP_WPS:
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ if (pLed->bLedWPSBlinkInProgress)
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ else
+ pLed->bLedWPSBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_WPS_STOP;
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ break;
+ case LED_CTL_STOP_WPS_FAIL:
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ break;
+ case LED_CTL_POWER_OFF:
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ SwLedOff(padapter, pLed);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
+}
+
+ /* Arcadyan/Sitecom , added by chiyoko, 20090216 */
+static void SwLedControlMode2(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+ switch (LedAction) {
+ case LED_CTL_SITE_SURVEY:
+ if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
+ } else if (!pLed->bLedScanBlinkInProgress) {
+ if (IS_LED_WPS_BLINKING(pLed))
+ return;
+
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedScanBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SCAN;
+ pLed->BlinkTimes = 24;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_TX:
+ case LED_CTL_RX:
+ if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ pLed->bLedBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_TXRX;
+ pLed->BlinkTimes = 2;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_LINK:
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ case LED_CTL_START_WPS: /* wait until xinpin finish */
+ case LED_CTL_START_WPS_BOTTON:
+ if (!pLed->bLedWPSBlinkInProgress) {
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ pLed->bLedWPSBlinkInProgress = true;
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ break;
+ case LED_CTL_STOP_WPS:
+ pLed->bLedWPSBlinkInProgress = false;
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ break;
+ case LED_CTL_STOP_WPS_FAIL:
+ pLed->bLedWPSBlinkInProgress = false;
+ if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
+ SwLedOff(padapter, pLed);
+ } else {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+ }
+ break;
+ case LED_CTL_START_TO_LINK:
+ case LED_CTL_NO_LINK:
+ if (!IS_LED_BLINKING(pLed)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ break;
+ case LED_CTL_POWER_OFF:
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
+}
+
+ /* COREGA, added by chiyoko, 20090316 */
+ static void SwLedControlMode3(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+ switch (LedAction) {
+ case LED_CTL_SITE_SURVEY:
+ if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
+ } else if (!pLed->bLedScanBlinkInProgress) {
+ if (IS_LED_WPS_BLINKING(pLed))
+ return;
+
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedScanBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SCAN;
+ pLed->BlinkTimes = 24;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_TX:
+ case LED_CTL_RX:
+ if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ pLed->bLedBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_TXRX;
+ pLed->BlinkTimes = 2;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_LINK:
+ if (IS_LED_WPS_BLINKING(pLed))
+ return;
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ case LED_CTL_START_WPS: /* wait until xinpin finish */
+ case LED_CTL_START_WPS_BOTTON:
+ if (!pLed->bLedWPSBlinkInProgress) {
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ pLed->bLedWPSBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_WPS;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_STOP_WPS:
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ } else {
+ pLed->bLedWPSBlinkInProgress = true;
+ }
+
+ pLed->CurrLedState = LED_BLINK_WPS_STOP;
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ break;
+ case LED_CTL_STOP_WPS_FAIL:
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ case LED_CTL_START_TO_LINK:
+ case LED_CTL_NO_LINK:
+ if (!IS_LED_BLINKING(pLed)) {
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ break;
+ case LED_CTL_POWER_OFF:
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ default:
+ break;
+ }
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
+ ("CurrLedState %d\n", pLed->CurrLedState));
+}
+
+ /* Edimax-Belkin, added by chiyoko, 20090413 */
+static void SwLedControlMode4(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct LED_871x *pLed = &(ledpriv->SwLed0);
+ struct LED_871x *pLed1 = &(ledpriv->SwLed1);
+
+ switch (LedAction) {
+ case LED_CTL_START_TO_LINK:
+ if (pLed1->bLedWPSBlinkInProgress) {
+ pLed1->bLedWPSBlinkInProgress = false;
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ pLed1->CurrLedState = RTW_LED_OFF;
+
+ if (pLed1->bLedOn)
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+
+ if (!pLed->bLedStartToLinkBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+
+ pLed->bLedStartToLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_StartToBlink;
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ }
+ }
+ break;
+ case LED_CTL_LINK:
+ case LED_CTL_NO_LINK:
+ /* LED1 settings */
+ if (LedAction == LED_CTL_LINK) {
+ if (pLed1->bLedWPSBlinkInProgress) {
+ pLed1->bLedWPSBlinkInProgress = false;
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ pLed1->CurrLedState = RTW_LED_OFF;
+
+ if (pLed1->bLedOn)
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+ }
+
+ if (!pLed->bLedNoLinkBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_SITE_SURVEY:
+ if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+ } else if (!pLed->bLedScanBlinkInProgress) {
+ if (IS_LED_WPS_BLINKING(pLed))
+ return;
+
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedScanBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SCAN;
+ pLed->BlinkTimes = 24;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_TX:
+ case LED_CTL_RX:
+ if (!pLed->bLedBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+ return;
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ pLed->bLedBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_TXRX;
+ pLed->BlinkTimes = 2;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_START_WPS: /* wait until xinpin finish */
+ case LED_CTL_START_WPS_BOTTON:
+ if (pLed1->bLedWPSBlinkInProgress) {
+ pLed1->bLedWPSBlinkInProgress = false;
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ pLed1->CurrLedState = RTW_LED_OFF;
+
+ if (pLed1->bLedOn)
+ _set_timer(&(pLed->BlinkTimer), 0);
+ }
+
+ if (!pLed->bLedWPSBlinkInProgress) {
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ pLed->bLedWPSBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_WPS;
+ if (pLed->bLedOn) {
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
+ } else {
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ }
+ }
+ break;
+ case LED_CTL_STOP_WPS: /* WPS connect success */
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+
+ break;
+ case LED_CTL_STOP_WPS_FAIL: /* WPS authentication fail */
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+
+ /* LED1 settings */
+ if (pLed1->bLedWPSBlinkInProgress)
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+ else
+ pLed1->bLedWPSBlinkInProgress = true;
+ pLed1->CurrLedState = LED_BLINK_WPS_STOP;
+ if (pLed1->bLedOn)
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed1->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ break;
+ case LED_CTL_STOP_WPS_FAIL_OVERLAP: /* WPS session overlap */
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ pLed->bLedNoLinkBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SLOWLY;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
+
+ /* LED1 settings */
+ if (pLed1->bLedWPSBlinkInProgress)
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+ else
+ pLed1->bLedWPSBlinkInProgress = true;
+ pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
+ pLed1->BlinkTimes = 10;
+ if (pLed1->bLedOn)
+ pLed1->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed1->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
+ break;
+ case LED_CTL_POWER_OFF:
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+
+ if (pLed->bLedNoLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedNoLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedLinkBlinkInProgress = false;
+ }
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ if (pLed->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedWPSBlinkInProgress = false;
+ }
+ if (pLed->bLedScanBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedScanBlinkInProgress = false;
+ }
+ if (pLed->bLedStartToLinkBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedStartToLinkBlinkInProgress = false;
+ }
+ if (pLed1->bLedWPSBlinkInProgress) {
+ _cancel_timer_ex(&(pLed1->BlinkTimer));
+ pLed1->bLedWPSBlinkInProgress = false;
+ }
+ pLed1->BlinkingLedState = LED_UNKNOWN;
+ SwLedOff(padapter, pLed);
+ SwLedOff(padapter, pLed1);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
+}
+
+ /* Sercomm-Belkin, added by chiyoko, 20090415 */
+static void
+SwLedControlMode5(
+ struct adapter *padapter,
+ enum LED_CTL_MODE LedAction
+)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+ switch (LedAction) {
+ case LED_CTL_POWER_ON:
+ case LED_CTL_NO_LINK:
+ case LED_CTL_LINK: /* solid blue */
+ pLed->CurrLedState = RTW_LED_ON;
+ pLed->BlinkingLedState = RTW_LED_ON;
+
+ _set_timer(&(pLed->BlinkTimer), 0);
+ break;
+ case LED_CTL_SITE_SURVEY:
+ if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+ } else if (!pLed->bLedScanBlinkInProgress) {
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ pLed->bLedScanBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_SCAN;
+ pLed->BlinkTimes = 24;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_TX:
+ case LED_CTL_RX:
+ if (!pLed->bLedBlinkInProgress) {
+ if (pLed->CurrLedState == LED_BLINK_SCAN)
+ return;
+ pLed->bLedBlinkInProgress = true;
+ pLed->CurrLedState = LED_BLINK_TXRX;
+ pLed->BlinkTimes = 2;
+ if (pLed->bLedOn)
+ pLed->BlinkingLedState = RTW_LED_OFF;
+ else
+ pLed->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
+ }
+ break;
+ case LED_CTL_POWER_OFF:
+ pLed->CurrLedState = RTW_LED_OFF;
+ pLed->BlinkingLedState = RTW_LED_OFF;
+
+ if (pLed->bLedBlinkInProgress) {
+ _cancel_timer_ex(&(pLed->BlinkTimer));
+ pLed->bLedBlinkInProgress = false;
+ }
+ SwLedOff(padapter, pLed);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
+}
+
+ /* WNC-Corega, added by chiyoko, 20090902 */
+static void
+SwLedControlMode6(
+ struct adapter *padapter,
+ enum LED_CTL_MODE LedAction
+)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct LED_871x *pLed0 = &(ledpriv->SwLed0);
+
+ switch (LedAction) {
+ case LED_CTL_POWER_ON:
+ case LED_CTL_LINK:
+ case LED_CTL_NO_LINK:
+ _cancel_timer_ex(&(pLed0->BlinkTimer));
+ pLed0->CurrLedState = RTW_LED_ON;
+ pLed0->BlinkingLedState = RTW_LED_ON;
+ _set_timer(&(pLed0->BlinkTimer), 0);
+ break;
+ case LED_CTL_POWER_OFF:
+ SwLedOff(padapter, pLed0);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("ledcontrol 6 Led %d\n", pLed0->CurrLedState));
+}
+
+/* */
+/* Description: */
+/* Handler function of LED Blinking. */
+/* We dispatch acture LED blink action according to LedStrategy. */
+/* */
+void BlinkHandler(struct LED_871x *pLed)
+{
+ struct adapter *padapter = pLed->padapter;
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+
+ if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
+ return;
+
+ switch (ledpriv->LedStrategy) {
+ case SW_LED_MODE0:
+ SwLedBlink(pLed);
+ break;
+ case SW_LED_MODE1:
+ SwLedBlink1(pLed);
+ break;
+ case SW_LED_MODE2:
+ SwLedBlink2(pLed);
+ break;
+ case SW_LED_MODE3:
+ SwLedBlink3(pLed);
+ break;
+ case SW_LED_MODE4:
+ SwLedBlink4(pLed);
+ break;
+ case SW_LED_MODE5:
+ SwLedBlink5(pLed);
+ break;
+ case SW_LED_MODE6:
+ SwLedBlink6(pLed);
+ break;
+ default:
+ break;
+ }
+}
+
+void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+ struct led_priv *ledpriv = &(padapter->ledpriv);
+ struct registry_priv *registry_par;
+
+ if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
+ (!padapter->hw_init_completed))
+ return;
+
+ if (!ledpriv->bRegUseLed)
+ return;
+
+ registry_par = &padapter->registrypriv;
+ if (!registry_par->led_enable)
+ return;
+
+ if ((padapter->pwrctrlpriv.rf_pwrstate != rf_on &&
+ padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) &&
+ (LedAction == LED_CTL_TX || LedAction == LED_CTL_RX ||
+ LedAction == LED_CTL_SITE_SURVEY ||
+ LedAction == LED_CTL_LINK ||
+ LedAction == LED_CTL_NO_LINK ||
+ LedAction == LED_CTL_POWER_ON))
+ return;
+
+ switch (ledpriv->LedStrategy) {
+ case SW_LED_MODE0:
+ break;
+ case SW_LED_MODE1:
+ SwLedControlMode1(padapter, LedAction);
+ break;
+ case SW_LED_MODE2:
+ SwLedControlMode2(padapter, LedAction);
+ break;
+ case SW_LED_MODE3:
+ SwLedControlMode3(padapter, LedAction);
+ break;
+ case SW_LED_MODE4:
+ SwLedControlMode4(padapter, LedAction);
+ break;
+ case SW_LED_MODE5:
+ SwLedControlMode5(padapter, LedAction);
+ break;
+ case SW_LED_MODE6:
+ SwLedControlMode6(padapter, LedAction);
+ break;
+ default:
+ break;
+ }
+
+ RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
+ ("LedStrategy:%d, LedAction %d\n",
+ ledpriv->LedStrategy, LedAction));
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mlme.c b/drivers/staging/r8188eu/core/rtw_mlme.c
new file mode 100644
index 000000000000..a30c2bd69313
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mlme.c
@@ -0,0 +1,2354 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MLME_C_
+
+#include <linux/version.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+#include <hal_intf.h>
+#include <mlme_osdep.h>
+#include <sta_info.h>
+#include <wifi.h>
+#include <wlan_bssdef.h>
+#include <rtw_ioctl_set.h>
+#include <usb_osintf.h>
+
+extern unsigned char MCS_rate_2R[16];
+extern unsigned char MCS_rate_1R[16];
+
+void rtw_set_roaming(struct adapter *adapter, u8 to_roaming)
+{
+ if (to_roaming == 0)
+ adapter->mlmepriv.to_join = false;
+ adapter->mlmepriv.to_roaming = to_roaming;
+}
+
+u8 rtw_to_roaming(struct adapter *adapter)
+{
+ return adapter->mlmepriv.to_roaming;
+}
+
+int _rtw_init_mlme_priv (struct adapter *padapter)
+{
+ int i;
+ u8 *pbuf;
+ struct wlan_network *pnetwork;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ int res = _SUCCESS;
+
+ /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+
+ pmlmepriv->nic_hdl = (u8 *)padapter;
+
+ pmlmepriv->pscanned = NULL;
+ pmlmepriv->fw_state = 0;
+ pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown;
+ pmlmepriv->scan_mode = SCAN_ACTIVE;/* 1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */
+
+ spin_lock_init(&(pmlmepriv->lock));
+ _rtw_init_queue(&(pmlmepriv->free_bss_pool));
+ _rtw_init_queue(&(pmlmepriv->scanned_queue));
+
+ set_scanned_network_val(pmlmepriv, 0);
+
+ memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+
+ pbuf = rtw_zvmalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+
+ if (pbuf == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+ pmlmepriv->free_bss_buf = pbuf;
+
+ pnetwork = (struct wlan_network *)pbuf;
+
+ for (i = 0; i < MAX_BSS_CNT; i++) {
+ INIT_LIST_HEAD(&(pnetwork->list));
+
+ list_add_tail(&(pnetwork->list), &(pmlmepriv->free_bss_pool.queue));
+
+ pnetwork++;
+ }
+
+ /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+ rtw_clear_scan_deny(padapter);
+
+ rtw_init_mlme_timer(padapter);
+
+exit:
+
+ return res;
+}
+
+static void rtw_mfree_mlme_priv_lock (struct mlme_priv *pmlmepriv)
+{
+ _rtw_spinlock_free(&pmlmepriv->lock);
+ _rtw_spinlock_free(&(pmlmepriv->free_bss_pool.lock));
+ _rtw_spinlock_free(&(pmlmepriv->scanned_queue.lock));
+}
+
+#if defined (CONFIG_88EU_AP_MODE)
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+ kfree(*ppie);
+ *plen = 0;
+ *ppie = NULL;
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+ rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+ rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+
+ rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+ rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+}
+#else
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+}
+#endif
+
+void _rtw_free_mlme_priv (struct mlme_priv *pmlmepriv)
+{
+
+ rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+ if (pmlmepriv) {
+ rtw_mfree_mlme_priv_lock (pmlmepriv);
+
+ if (pmlmepriv->free_bss_buf) {
+ rtw_vmfree(pmlmepriv->free_bss_buf, MAX_BSS_CNT * sizeof(struct wlan_network));
+ }
+ }
+
+}
+
+int _rtw_enqueue_network(struct __queue *queue, struct wlan_network *pnetwork)
+{
+
+ if (pnetwork == NULL)
+ goto exit;
+
+ spin_lock_bh(&queue->lock);
+
+ list_add_tail(&pnetwork->list, &queue->queue);
+
+ spin_unlock_bh(&queue->lock);
+
+exit:
+
+ return _SUCCESS;
+}
+
+struct wlan_network *_rtw_dequeue_network(struct __queue *queue)
+{
+ struct wlan_network *pnetwork;
+
+ spin_lock_bh(&queue->lock);
+
+ if (list_empty(&queue->queue)) {
+ pnetwork = NULL;
+ } else {
+ pnetwork = container_of((&queue->queue)->next, struct wlan_network, list);
+
+ list_del_init(&(pnetwork->list));
+ }
+
+ spin_unlock_bh(&queue->lock);
+
+ return pnetwork;
+}
+
+struct wlan_network *_rtw_alloc_network(struct mlme_priv *pmlmepriv)/* _queue *free_queue) */
+{
+ struct wlan_network *pnetwork;
+ struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+ struct list_head *plist = NULL;
+
+ spin_lock_bh(&free_queue->lock);
+
+ if (list_empty(&free_queue->queue)) {
+ pnetwork = NULL;
+ goto exit;
+ }
+ plist = (&(free_queue->queue))->next;
+
+ pnetwork = container_of(plist , struct wlan_network, list);
+
+ list_del_init(&pnetwork->list);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("_rtw_alloc_network: ptr=%p\n", plist));
+ pnetwork->network_type = 0;
+ pnetwork->fixed = false;
+ pnetwork->last_scanned = jiffies;
+ pnetwork->aid = 0;
+ pnetwork->join_res = 0;
+
+ pmlmepriv->num_of_scanned++;
+
+exit:
+ spin_unlock_bh(&free_queue->lock);
+
+ return pnetwork;
+}
+
+void _rtw_free_network(struct mlme_priv *pmlmepriv , struct wlan_network *pnetwork, u8 isfreeall)
+{
+ u32 curr_time, delta_time;
+ u32 lifetime = SCANQUEUE_LIFETIME;
+ struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+ if (pnetwork == NULL)
+ return;
+
+ if (pnetwork->fixed)
+ return;
+ curr_time = jiffies;
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+ lifetime = 1;
+ if (!isfreeall) {
+ delta_time = (curr_time - pnetwork->last_scanned)/HZ;
+ if (delta_time < lifetime)/* unit:sec */
+ return;
+ }
+ spin_lock_bh(&free_queue->lock);
+ list_del_init(&(pnetwork->list));
+ list_add_tail(&(pnetwork->list), &(free_queue->queue));
+ pmlmepriv->num_of_scanned--;
+ spin_unlock_bh(&free_queue->lock);
+}
+
+void _rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+ struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+ if (pnetwork == NULL)
+ return;
+ if (pnetwork->fixed)
+ return;
+ list_del_init(&(pnetwork->list));
+ list_add_tail(&(pnetwork->list), get_list_head(free_queue));
+ pmlmepriv->num_of_scanned--;
+}
+
+/*
+ return the wlan_network with the matching addr
+
+ Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+ struct list_head *phead, *plist;
+ struct wlan_network *pnetwork = NULL;
+ u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+ if (!memcmp(zero_addr, addr, ETH_ALEN)) {
+ pnetwork = NULL;
+ goto exit;
+ }
+ phead = get_list_head(scanned_queue);
+ plist = phead->next;
+
+ while (plist != phead) {
+ pnetwork = container_of(plist, struct wlan_network , list);
+ if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
+ break;
+ plist = plist->next;
+ }
+ if (plist == phead)
+ pnetwork = NULL;
+exit:
+
+ return pnetwork;
+}
+
+void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall)
+{
+ struct list_head *phead, *plist;
+ struct wlan_network *pnetwork;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct __queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+ spin_lock_bh(&scanned_queue->lock);
+
+ phead = get_list_head(scanned_queue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ pnetwork = container_of(plist, struct wlan_network, list);
+
+ plist = plist->next;
+
+ _rtw_free_network(pmlmepriv, pnetwork, isfreeall);
+ }
+ spin_unlock_bh(&scanned_queue->lock);
+
+}
+
+int rtw_if_up(struct adapter *padapter)
+{
+ int res;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+ (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("rtw_if_up:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
+ padapter->bDriverStopped, padapter->bSurpriseRemoved));
+ res = false;
+ } else {
+ res = true;
+ }
+
+ return res;
+}
+
+void rtw_generate_random_ibss(u8 *pibss)
+{
+ u32 curtime = jiffies;
+
+ pibss[0] = 0x02; /* in ad-hoc mode bit1 must set to 1 */
+ pibss[1] = 0x11;
+ pibss[2] = 0x87;
+ pibss[3] = (u8)(curtime & 0xff);/* p[0]; */
+ pibss[4] = (u8)((curtime>>8) & 0xff);/* p[1]; */
+ pibss[5] = (u8)((curtime>>16) & 0xff);/* p[2]; */
+
+ return;
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+ return ie + 8 + 2;
+}
+
+u16 rtw_get_capability(struct wlan_bssid_ex *bss)
+{
+ __le16 val;
+
+ memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2);
+
+ return le16_to_cpu(val);
+}
+
+u8 *rtw_get_timestampe_from_ie(u8 *ie)
+{
+ return ie + 0;
+}
+
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie)
+{
+ return ie + 8;
+}
+
+int rtw_init_mlme_priv (struct adapter *padapter)/* struct mlme_priv *pmlmepriv) */
+{
+ int res;
+
+ res = _rtw_init_mlme_priv(padapter);/* (pmlmepriv); */
+
+ return res;
+}
+
+void rtw_free_mlme_priv (struct mlme_priv *pmlmepriv)
+{
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv\n"));
+ _rtw_free_mlme_priv (pmlmepriv);
+
+}
+
+static struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
+{
+ struct wlan_network *pnetwork;
+
+ pnetwork = _rtw_alloc_network(pmlmepriv);
+
+ return pnetwork;
+}
+
+static void rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+ struct wlan_network *pnetwork)
+{
+
+ _rtw_free_network_nolock(pmlmepriv, pnetwork);
+
+}
+
+void rtw_free_network_queue(struct adapter *dev, u8 isfreeall)
+{
+
+ _rtw_free_network_queue(dev, isfreeall);
+
+}
+
+/*
+ return the wlan_network with the matching addr
+
+ Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+ struct wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+ return pnetwork;
+}
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+ int ret = true;
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+ if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
+ (pnetwork->network.Privacy == 0))
+ ret = false;
+ else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
+ (pnetwork->network.Privacy == 1))
+ ret = false;
+ else
+ ret = true;
+ return ret;
+}
+
+static int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
+{
+ return (a->Ssid.SsidLength == b->Ssid.SsidLength) &&
+ !memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength);
+}
+
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
+{
+ u16 s_cap, d_cap;
+ __le16 le_scap, le_dcap;
+
+ memcpy((u8 *)&le_scap, rtw_get_capability_from_ie(src->IEs), 2);
+ memcpy((u8 *)&le_dcap, rtw_get_capability_from_ie(dst->IEs), 2);
+
+ s_cap = le16_to_cpu(le_scap);
+ d_cap = le16_to_cpu(le_dcap);
+
+ return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+ ((!memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN))) &&
+ ((!memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength))) &&
+ ((s_cap & WLAN_CAPABILITY_IBSS) ==
+ (d_cap & WLAN_CAPABILITY_IBSS)) &&
+ ((s_cap & WLAN_CAPABILITY_BSS) ==
+ (d_cap & WLAN_CAPABILITY_BSS)));
+}
+
+struct wlan_network *rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
+{
+ struct list_head *plist, *phead;
+ struct wlan_network *pwlan = NULL;
+ struct wlan_network *oldest = NULL;
+
+ phead = get_list_head(scanned_queue);
+
+ plist = phead->next;
+
+ while (1) {
+ if (phead == plist)
+ break;
+
+ pwlan = container_of(plist, struct wlan_network, list);
+
+ if (!pwlan->fixed) {
+ if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
+ oldest = pwlan;
+ }
+
+ plist = plist->next;
+ }
+
+ return oldest;
+}
+
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+ struct adapter *padapter, bool update_ie)
+{
+ long rssi_ori = dst->Rssi;
+ u8 sq_smp = src->PhyInfo.SignalQuality;
+ u8 ss_final;
+ u8 sq_final;
+ long rssi_final;
+
+ rtw_hal_antdiv_rssi_compared(padapter, dst, src); /* this will update src.Rssi, need consider again */
+
+ /* The rule below is 1/5 for sample value, 4/5 for history value */
+ if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src)) {
+ /* Take the recvpriv's value for the connected AP*/
+ ss_final = padapter->recvpriv.signal_strength;
+ sq_final = padapter->recvpriv.signal_qual;
+ /* the rssi value here is undecorated, and will be used for antenna diversity */
+ if (sq_smp != 101) /* from the right channel */
+ rssi_final = dst->Rssi; //(src->Rssi+dst->Rssi*4)/5;
+ else
+ rssi_final = rssi_ori;
+ } else {
+// if (sq_smp != 101) { /* from the right channel */
+ ss_final = (u32)dst->PhyInfo.SignalStrength; //((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
+ sq_final = (u32)dst->PhyInfo.SignalQuality; //((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
+ rssi_final = dst->Rssi; //(src->Rssi+dst->Rssi*4)/5;
+// } else {
+// /* bss info not receiving from the right channel, use the original RX signal infos */
+// ss_final = dst->PhyInfo.SignalStrength;
+// sq_final = dst->PhyInfo.SignalQuality;
+// rssi_final = dst->Rssi;
+// }
+ }
+ if (update_ie) {
+ dst->Reserved[0] = src->Reserved[0];
+ dst->Reserved[1] = src->Reserved[1];
+ memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src));
+ }
+ dst->PhyInfo.SignalStrength = ss_final;
+ dst->PhyInfo.SignalQuality = sq_final;
+ dst->Rssi = rssi_final;
+
+}
+
+static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+ if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) &&
+ (is_same_network(&(pmlmepriv->cur_network.network), pnetwork))) {
+ update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true);
+ rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(struct ndis_802_11_fixed_ie),
+ pmlmepriv->cur_network.network.IELength);
+ }
+
+}
+
+/*
+Caller must hold pmlmepriv->lock first.
+*/
+void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
+{
+ struct list_head *plist, *phead;
+ u32 bssid_ex_sz;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct __queue *queue = &(pmlmepriv->scanned_queue);
+ struct wlan_network *pnetwork = NULL;
+ struct wlan_network *oldest = NULL;
+
+ spin_lock_bh(&queue->lock);
+ phead = get_list_head(queue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ pnetwork = container_of(plist, struct wlan_network, list);
+
+ if (is_same_network(&(pnetwork->network), target))
+ break;
+ if ((oldest == ((struct wlan_network *)0)) ||
+ time_after(oldest->last_scanned, pnetwork->last_scanned))
+ oldest = pnetwork;
+ plist = plist->next;
+ }
+ /* If we didn't find a match, then get a new network slot to initialize
+ * with this beacon's information */
+ if (phead == plist) {
+ if (list_empty(&(pmlmepriv->free_bss_pool.queue))) {
+ /* If there are no more slots, expire the oldest */
+ pnetwork = oldest;
+
+ rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+ memcpy(&(pnetwork->network), target, get_wlan_bssid_ex_sz(target));
+ /* variable initialize */
+ pnetwork->fixed = false;
+ pnetwork->last_scanned = jiffies;
+
+ pnetwork->network_type = 0;
+ pnetwork->aid = 0;
+ pnetwork->join_res = 0;
+
+ /* bss info not receiving from the right channel */
+ if (pnetwork->network.PhyInfo.SignalQuality == 101)
+ pnetwork->network.PhyInfo.SignalQuality = 0;
+ } else {
+ /* Otherwise just pull from the free list */
+
+ pnetwork = rtw_alloc_network(pmlmepriv); /* will update scan_time */
+
+ if (pnetwork == NULL) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
+ goto exit;
+ }
+
+ bssid_ex_sz = get_wlan_bssid_ex_sz(target);
+ target->Length = bssid_ex_sz;
+ rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+ memcpy(&(pnetwork->network), target, bssid_ex_sz);
+
+ pnetwork->last_scanned = jiffies;
+
+ /* bss info not receiving from the right channel */
+ if (pnetwork->network.PhyInfo.SignalQuality == 101)
+ pnetwork->network.PhyInfo.SignalQuality = 0;
+ list_add_tail(&(pnetwork->list), &(queue->queue));
+ }
+ } else {
+ /* we have an entry and we are going to update it. But this entry may
+ * be already expired. In this case we do the same as we found a new
+ * net and call the new_net handler
+ */
+ bool update_ie = true;
+
+ pnetwork->last_scanned = jiffies;
+
+ /* target.Reserved[0]== 1, means that scanned network is a bcn frame. */
+ /* probe resp(3) > beacon(1) > probe req(2) */
+ if ((target->Reserved[0] != 2) &&
+ (target->Reserved[0] >= pnetwork->network.Reserved[0]))
+ update_ie = true;
+ else
+ update_ie = false;
+ update_network(&(pnetwork->network), target, adapter, update_ie);
+ }
+
+exit:
+ spin_unlock_bh(&queue->lock);
+
+}
+
+static void rtw_add_network(struct adapter *adapter,
+ struct wlan_bssid_ex *pnetwork)
+{
+
+#if defined(CONFIG_88EU_P2P)
+ rtw_wlan_bssid_ex_remove_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO);
+#endif
+ update_current_network(adapter, pnetwork);
+ rtw_update_scanned_network(adapter, pnetwork);
+
+}
+
+/* select the desired network based on the capability of the (i)bss. */
+/* check items: (1) security */
+/* (2) network_type */
+/* (3) WMM */
+/* (4) HT */
+/* (5) others */
+static int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ u32 desired_encmode;
+ u32 privacy;
+
+ /* u8 wps_ie[512]; */
+ uint wps_ielen;
+
+ int bselected = true;
+
+ desired_encmode = psecuritypriv->ndisencryptstatus;
+ privacy = pnetwork->network.Privacy;
+
+ if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+ if (rtw_get_wps_ie(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL)
+ return true;
+ else
+ return false;
+ }
+ if (adapter->registrypriv.wifi_spec == 1) { /* for correct flow of 8021X to do.... */
+ u8 *p = NULL;
+ uint ie_len = 0;
+
+ if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+ bselected = false;
+ if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) {
+ p = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_,
+ _RSN_IE_2_, &ie_len,
+ (pnetwork->network.IELength -
+ _BEACON_IE_OFFSET_));
+ if (p && ie_len > 0)
+ bselected = true;
+ else
+ bselected = false;
+ }
+ }
+
+ if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) {
+ DBG_88E("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+ bselected = false;
+ }
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+ if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+ bselected = false;
+ }
+
+ return bselected;
+}
+
+/* TODO: Perry: For Power Management */
+void rtw_atimdone_event_callback(struct adapter *adapter , u8 *pbuf)
+{
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("receive atimdone_evet\n"));
+
+ return;
+}
+
+void rtw_survey_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+ u32 len;
+ struct wlan_bssid_ex *pnetwork;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+ pnetwork = (struct wlan_bssid_ex *)pbuf;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_survey_event_callback, ssid=%s\n", pnetwork->Ssid.Ssid));
+
+ len = get_wlan_bssid_ex_sz(pnetwork);
+ if (len > (sizeof(struct wlan_bssid_ex))) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n****rtw_survey_event_callback: return a wrong bss ***\n"));
+ return;
+ }
+ spin_lock_bh(&pmlmepriv->lock);
+
+ /* update IBSS_network 's timestamp */
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) {
+ if (!memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN)) {
+ struct wlan_network *ibss_wlan = NULL;
+
+ memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->MacAddress);
+ if (ibss_wlan) {
+ memcpy(ibss_wlan->network.IEs , pnetwork->IEs, 8);
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ goto exit;
+ }
+ spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+ }
+ }
+
+ /* lock pmlmepriv->lock when you accessing network_q */
+ if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == false) {
+ if (pnetwork->Ssid.Ssid[0] == 0)
+ pnetwork->Ssid.SsidLength = 0;
+ rtw_add_network(adapter, pnetwork);
+ }
+
+exit:
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ return;
+}
+
+void rtw_surveydone_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext;
+ u8 timer_cancelled = 0;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (pmlmepriv->wps_probe_req_ie) {
+ pmlmepriv->wps_probe_req_ie_len = 0;
+ kfree(pmlmepriv->wps_probe_req_ie);
+ pmlmepriv->wps_probe_req_ie = NULL;
+ }
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+ timer_cancelled = 1;
+
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+ } else {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status=%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));
+ }
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ if (timer_cancelled)
+ _cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+
+ spin_lock_bh(&pmlmepriv->lock);
+ rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+ if (pmlmepriv->to_join) {
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+ set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+ if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) {
+ _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+ } else {
+ struct wlan_bssid_ex *pdev_network = &(adapter->registrypriv.dev_network);
+ u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
+
+ memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+ memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+ rtw_update_registrypriv_dev_network(adapter);
+ rtw_generate_random_ibss(pibss);
+
+ pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+ if (rtw_createbss_cmd(adapter) != _SUCCESS)
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error=>rtw_createbss_cmd status FAIL\n"));
+ pmlmepriv->to_join = false;
+ }
+ }
+ } else {
+ int s_ret;
+ set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+ pmlmepriv->to_join = false;
+ s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+ if (_SUCCESS == s_ret) {
+ _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+ } else if (s_ret == 2) { /* there is no need to wait for join */
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+ rtw_indicate_connect(adapter);
+ } else {
+ DBG_88E("try_to_join, but select scanning queue fail, to_roaming:%d\n",
+ pmlmepriv->to_roaming);
+ if (rtw_to_roaming(adapter) != 0) {
+ if (--pmlmepriv->to_roaming == 0 ||
+ _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)) {
+ rtw_set_roaming(adapter, 0);
+ rtw_free_assoc_resources(adapter, 1);
+ rtw_indicate_disconnect(adapter);
+ } else {
+ pmlmepriv->to_join = true;
+ }
+ } else {
+ rtw_indicate_disconnect(adapter);
+ }
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+ }
+ }
+ }
+
+ indicate_wx_scan_complete_event(adapter);
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+ p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0);
+
+ rtw_os_xmit_schedule(adapter);
+
+ pmlmeext = &adapter->mlmeextpriv;
+
+}
+
+void rtw_dummy_event_callback(struct adapter *adapter , u8 *pbuf)
+{
+}
+
+void rtw_fwdbg_event_callback(struct adapter *adapter , u8 *pbuf)
+{
+}
+
+static void free_scanqueue(struct mlme_priv *pmlmepriv)
+{
+ struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+ struct __queue *scan_queue = &pmlmepriv->scanned_queue;
+ struct list_head *plist, *phead, *ptemp;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n"));
+ spin_lock_bh(&scan_queue->lock);
+ spin_lock_bh(&free_queue->lock);
+
+ phead = get_list_head(scan_queue);
+ plist = phead->next;
+
+ while (plist != phead) {
+ ptemp = plist->next;
+ list_del_init(plist);
+ list_add_tail(plist, &free_queue->queue);
+ plist = ptemp;
+ pmlmepriv->num_of_scanned--;
+ }
+
+ spin_unlock_bh(&free_queue->lock);
+ spin_unlock_bh(&scan_queue->lock);
+}
+
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
+{
+ struct wlan_network *pwlan = NULL;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources\n"));
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("tgt_network->network.MacAddress=%pM ssid=%s\n",
+ tgt_network->network.MacAddress, tgt_network->network.Ssid.Ssid));
+
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_AP_STATE)) {
+ struct sta_info *psta;
+
+ psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(adapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+ }
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE)) {
+ struct sta_info *psta;
+
+ rtw_free_all_stainfo(adapter);
+
+ psta = rtw_get_bcmc_stainfo(adapter);
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(adapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ rtw_init_bcmc_stainfo(adapter);
+ }
+
+ if (lock_scanned_queue)
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+ pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+ if (pwlan)
+ pwlan->fixed = false;
+ else
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_assoc_resources:pwlan==NULL\n\n"));
+
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1)))
+ rtw_free_network_nolock(pmlmepriv, pwlan);
+
+ if (lock_scanned_queue)
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ pmlmepriv->key_mask = 0;
+
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect\n"));
+
+ pmlmepriv->to_join = false;
+
+ if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+ set_fwstate(pmlmepriv, _FW_LINKED);
+
+ rtw_led_control(padapter, LED_CTL_LINK);
+
+ rtw_os_indicate_connect(padapter);
+ }
+
+ pmlmepriv->to_roaming = 0;
+
+ rtw_set_scan_deny(padapter, 3000);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect: fw_state=0x%08x\n", get_fwstate(pmlmepriv)));
+
+}
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n"));
+
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING | WIFI_UNDER_WPS);
+
+ if (pmlmepriv->to_roaming > 0)
+ _clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+ if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) ||
+ (pmlmepriv->to_roaming <= 0)) {
+ rtw_os_indicate_disconnect(padapter);
+
+ _clr_fwstate_(pmlmepriv, _FW_LINKED);
+ rtw_led_control(padapter, LED_CTL_NO_LINK);
+ rtw_clear_scan_deny(padapter);
+ }
+ p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+
+ rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+
+}
+
+inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
+{
+ rtw_os_indicate_scan_done(padapter, aborted);
+}
+
+void rtw_scan_abort(struct adapter *adapter)
+{
+ u32 start;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv);
+
+ start = jiffies;
+ pmlmeext->scan_abort = true;
+ while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) &&
+ rtw_get_passing_time_ms(start) <= 200) {
+ if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+ break;
+ DBG_88E(FUNC_NDEV_FMT"fw_state=_FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+ rtw_msleep_os(20);
+ }
+ if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+ if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
+ DBG_88E(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+ rtw_indicate_scan_done(adapter, true);
+ }
+ pmlmeext->scan_abort = false;
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+ int i;
+ struct sta_info *bmc_sta, *psta = NULL;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress);
+ if (psta == NULL)
+ psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
+
+ if (psta) { /* update ptarget_sta */
+ DBG_88E("%s\n", __func__);
+ psta->aid = pnetwork->join_res;
+ psta->mac_id = 0;
+ /* sta mode */
+ rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+ /* security related */
+ if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+ padapter->securitypriv.binstallGrpkey = false;
+ padapter->securitypriv.busetkipkey = false;
+ padapter->securitypriv.bgrpkey_handshake = false;
+ psta->ieee8021x_blocked = true;
+ psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+ memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype));
+ memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype));
+ memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype));
+ memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48));
+ memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+ }
+ /* Commented by Albert 2012/07/21 */
+ /* When doing the WPS, the wps_ie_len won't equal to 0 */
+ /* And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
+ if (padapter->securitypriv.wps_ie_len != 0) {
+ psta->ieee8021x_blocked = true;
+ padapter->securitypriv.wps_ie_len = 0;
+ }
+ /* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
+ /* if A-MPDU Rx is enabled, resetting rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+ /* todo: check if AP can send A-MPDU packets */
+ for (i = 0; i < 16; i++) {
+ /* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+ preorder_ctrl = &psta->recvreorder_ctrl[i];
+ preorder_ctrl->enable = false;
+ preorder_ctrl->indicate_seq = 0xffff;
+ preorder_ctrl->wend_b = 0xffff;
+ preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+ }
+ bmc_sta = rtw_get_bcmc_stainfo(padapter);
+ if (bmc_sta) {
+ for (i = 0; i < 16; i++) {
+ /* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+ preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+ preorder_ctrl->enable = false;
+ preorder_ctrl->indicate_seq = 0xffff;
+ preorder_ctrl->wend_b = 0xffff;
+ preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+ }
+ }
+ /* misc. */
+ update_sta_info(padapter, psta);
+ }
+ return psta;
+}
+
+/* pnetwork: returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network *pnetwork)
+{
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+
+ DBG_88E("%s\n", __func__);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("\nfw_state:%x, BSSID:%pM\n",
+ get_fwstate(pmlmepriv), pnetwork->network.MacAddress));
+
+ /* why not use ptarget_wlan?? */
+ memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+ /* some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */
+ cur_network->network.IELength = ptarget_wlan->network.IELength;
+ memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+
+ cur_network->aid = pnetwork->join_res;
+
+ rtw_set_signal_stat_timer(&padapter->recvpriv);
+ padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
+ padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
+ /* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
+ padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+ rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+ /* update fw_state will clr _FW_UNDER_LINKING here indirectly */
+ switch (pnetwork->network.InfrastructureMode) {
+ case Ndis802_11Infrastructure:
+ if (pmlmepriv->fw_state&WIFI_UNDER_WPS)
+ pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
+ else
+ pmlmepriv->fw_state = WIFI_STATION_STATE;
+ break;
+ case Ndis802_11IBSS:
+ pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+ break;
+ default:
+ pmlmepriv->fw_state = WIFI_NULL_STATE;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n"));
+ break;
+ }
+
+ rtw_update_protection(padapter, (cur_network->network.IEs) +
+ sizeof(struct ndis_802_11_fixed_ie),
+ (cur_network->network.IELength));
+ rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength);
+}
+
+/* Notes: the function could be > passive_level (the same context as Rx tasklet) */
+/* pnetwork: returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+/* if join_res > 0, for (fw_state == WIFI_STATION_STATE), we check if "ptarget_sta" & "ptarget_wlan" exist. */
+/* if join_res > 0, for (fw_state == WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */
+/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan != NULL). */
+
+void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
+{
+ u8 timer_cancelled;
+ struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
+ struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+ struct wlan_network *pcur_wlan = NULL, *ptarget_wlan = NULL;
+ unsigned int the_same_macaddr = false;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("joinbss event call back received with res=%d\n", pnetwork->join_res));
+
+ rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+
+ if (pmlmepriv->assoc_ssid.SsidLength == 0)
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@ joinbss event call back for Any SSid\n"));
+ else
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@ rtw_joinbss_event_callback for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+
+ the_same_macaddr = !memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN);
+
+ pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
+ if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+ return;
+ }
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nrtw_joinbss_event_callback!! spin_lock_init\n"));
+
+ if (pnetwork->join_res > 0) {
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+ /* s1. find ptarget_wlan */
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ if (the_same_macaddr) {
+ ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+ } else {
+ pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+ if (pcur_wlan)
+ pcur_wlan->fixed = false;
+
+ pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+ if (pcur_sta) {
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ rtw_free_stainfo(adapter, pcur_sta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+ }
+
+ ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+ if (ptarget_wlan)
+ ptarget_wlan->fixed = true;
+ }
+ }
+ } else {
+ ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+ if (ptarget_wlan)
+ ptarget_wlan->fixed = true;
+ }
+ }
+
+ /* s2. update cur_network */
+ if (ptarget_wlan) {
+ rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+ } else {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't find ptarget_wlan when joinbss_event callback\n"));
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ goto ignore_joinbss_callback;
+ }
+
+ /* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+ ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+ if (ptarget_sta == NULL) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't update stainfo when joinbss_event callback\n"));
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ goto ignore_joinbss_callback;
+ }
+ }
+
+ /* s4. indicate connect */
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+ pmlmepriv->cur_network_scanned = ptarget_wlan;
+ rtw_indicate_connect(adapter);
+ } else {
+ /* adhoc mode will rtw_indicate_connect when rtw_stassoc_event_callback */
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+ }
+
+ /* s5. Cancle assoc_timer */
+ _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n"));
+
+ } else {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ goto ignore_joinbss_callback;
+ }
+
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+ } else if (pnetwork->join_res == -4) {
+ rtw_reset_securitypriv(adapter);
+ _set_timer(&pmlmepriv->assoc_timer, 1);
+
+ if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n", get_fwstate(pmlmepriv)));
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+ }
+ } else { /* if join_res < 0 (join fails), then try again */
+ _set_timer(&pmlmepriv->assoc_timer, 1);
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+ }
+
+ignore_joinbss_callback:
+ spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+ struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
+
+ mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+ rtw_os_xmit_schedule(adapter);
+
+}
+
+static u8 search_max_mac_id(struct adapter *padapter)
+{
+ u8 mac_id;
+#if defined (CONFIG_88EU_AP_MODE)
+ u8 aid;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+#endif
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#if defined (CONFIG_88EU_AP_MODE)
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ for (aid = (pstapriv->max_num_sta); aid > 0; aid--) {
+ if (pstapriv->sta_aid[aid-1] != NULL)
+ break;
+ }
+ mac_id = aid + 1;
+ } else
+#endif
+ {/* adhoc id = 31~2 */
+ for (mac_id = (NUM_STA-1); mac_id >= IBSS_START_MAC_ID; mac_id--) {
+ if (pmlmeinfo->FW_sta_info[mac_id].status == 1)
+ break;
+ }
+ }
+ return mac_id;
+}
+
+/* FOR AP , AD-HOC mode */
+void rtw_sta_media_status_rpt(struct adapter *adapter,struct sta_info *psta,
+ u32 mstatus)
+{
+ u16 media_status_rpt;
+ u8 macid;
+
+ if (psta == NULL)
+ return;
+
+ macid = search_max_mac_id(adapter);
+ rtw_hal_set_hwreg(adapter, HW_VAR_TX_RPT_MAX_MACID, (u8 *)&macid);
+ /* MACID|OPMODE:1 connect */
+ media_status_rpt = (u16)((psta->mac_id<<8) | mstatus);
+ rtw_hal_set_hwreg(adapter,HW_VAR_H2C_MEDIA_STATUS_RPT,
+ (u8 *)&media_status_rpt);
+}
+
+void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+ struct sta_info *psta;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct stassoc_event *pstassoc = (struct stassoc_event *)pbuf;
+ struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+ struct wlan_network *ptarget_wlan = NULL;
+
+ if (rtw_access_ctrl(adapter, pstassoc->macaddr) == false)
+ return;
+
+#if defined (CONFIG_88EU_AP_MODE)
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+ if (psta)
+ rtw_indicate_sta_assoc_event(adapter, psta);
+ return;
+ }
+#endif
+ /* for AD-HOC mode */
+ psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+ if (psta != NULL) {
+ /* the sta have been in sta_info_queue => do nothing */
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue\n"));
+ return; /* between drv has received this event before and fw have not yet to set key to CAM_ENTRY) */
+ }
+ psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);
+ if (psta == NULL) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't alloc sta_info when rtw_stassoc_event_callback\n"));
+ return;
+ }
+ /* to do: init sta_info variable */
+ psta->qos_option = 0;
+ psta->mac_id = (uint)pstassoc->cam_id;
+ DBG_88E("%s\n", __func__);
+ /* for ad-hoc mode */
+ rtw_hal_set_odm_var(adapter, HAL_ODM_STA_INFO, psta, true);
+ rtw_sta_media_status_rpt(adapter, psta, 1);
+ if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+ psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+ psta->ieee8021x_blocked = false;
+ spin_lock_bh(&pmlmepriv->lock);
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))) {
+ if (adapter->stapriv.asoc_sta_count == 2) {
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+ pmlmepriv->cur_network_scanned = ptarget_wlan;
+ if (ptarget_wlan)
+ ptarget_wlan->fixed = true;
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+ rtw_indicate_connect(adapter);
+ }
+ }
+ spin_unlock_bh(&pmlmepriv->lock);
+ mlmeext_sta_add_event_callback(adapter, psta);
+}
+
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+ int mac_id = -1;
+ struct sta_info *psta;
+ struct wlan_network *pwlan = NULL;
+ struct wlan_bssid_ex *pdev_network = NULL;
+ u8 *pibss = NULL;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ struct stadel_event *pstadel = (struct stadel_event *)pbuf;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+
+ psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+ if (psta)
+ mac_id = psta->mac_id;
+ else
+ mac_id = pstadel->mac_id;
+
+ DBG_88E("%s(mac_id=%d)=%pM\n", __func__, mac_id, pstadel->macaddr);
+
+ if (mac_id >= 0) {
+ u16 media_status;
+ media_status = (mac_id<<8)|0; /* MACID|OPMODE:0 means disconnect */
+ /* for STA, AP, ADHOC mode, report disconnect stauts to FW */
+ rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
+ }
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+ return;
+
+ mlmeext_sta_del_event_callback(adapter);
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+ if(adapter->registrypriv.wifi_spec == 1)
+ rtw_set_roaming(adapter, 0); /* don't roam */
+ else if (rtw_to_roaming(adapter) > 0)
+ pmlmepriv->to_roaming--; /* this stadel_event is caused by roaming, decrease to_roaming */
+ else if (rtw_to_roaming(adapter) == 0)
+ rtw_set_roaming(adapter,
+ adapter->registrypriv.max_roaming_times);
+
+ if (*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+ rtw_set_roaming(adapter, 0); /* don't roam */
+
+ rtw_free_uc_swdec_pending_queue(adapter);
+
+ rtw_free_assoc_resources(adapter, 1);
+ rtw_indicate_disconnect(adapter);
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ /* remove the network entry in scanned_queue */
+ pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+ if (pwlan) {
+ pwlan->fixed = false;
+ rtw_free_network_nolock(pmlmepriv, pwlan);
+ }
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ _rtw_roaming(adapter, tgt_network);
+ }
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+ spin_lock_bh(&(pstapriv->sta_hash_lock));
+ rtw_free_stainfo(adapter, psta);
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ if (adapter->stapriv.asoc_sta_count == 1) { /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ /* free old ibss network */
+ pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+ if (pwlan) {
+ pwlan->fixed = false;
+ rtw_free_network_nolock(pmlmepriv, pwlan);
+ }
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+ /* re-create ibss */
+ pdev_network = &(adapter->registrypriv.dev_network);
+ pibss = adapter->registrypriv.dev_network.MacAddress;
+
+ memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+
+ memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+ memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+ rtw_update_registrypriv_dev_network(adapter);
+
+ rtw_generate_random_ibss(pibss);
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+ set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+ _clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+ }
+
+ if (rtw_createbss_cmd(adapter) != _SUCCESS)
+ RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error=>stadel_event_callback: rtw_createbss_cmd status FAIL***\n "));
+ }
+ }
+ spin_unlock_bh(&pmlmepriv->lock);
+
+}
+
+void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_cpwm_event_callback !!!\n"));
+
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler (struct adapter *adapter)
+{
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ int do_join_r;
+
+ DBG_88E("%s, fw_state=%x\n", __func__, get_fwstate(pmlmepriv));
+
+ if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+ return;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (rtw_to_roaming(adapter) > 0) { /* join timeout caused by roaming */
+ while (1) {
+ pmlmepriv->to_roaming--;
+ if (rtw_to_roaming(adapter) != 0) { /* try another */
+ DBG_88E("%s try another roaming\n", __func__);
+ do_join_r = rtw_do_join(adapter);
+ if (_SUCCESS != do_join_r) {
+ DBG_88E("%s roaming do_join return %d\n", __func__ , do_join_r);
+ continue;
+ }
+ break;
+ } else {
+ DBG_88E("%s We've try roaming but fail\n", __func__);
+ rtw_indicate_disconnect(adapter);
+ break;
+ }
+ }
+ } else {
+ rtw_indicate_disconnect(adapter);
+ free_scanqueue(pmlmepriv);/* */
+ }
+ spin_unlock_bh(&pmlmepriv->lock);
+
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler (struct adapter *adapter)
+{
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+ DBG_88E(FUNC_ADPT_FMT" fw_state=%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+ spin_lock_bh(&pmlmepriv->lock);
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+ spin_unlock_bh(&pmlmepriv->lock);
+ rtw_indicate_scan_done(adapter, true);
+}
+
+static void rtw_auto_scan_handler(struct adapter *padapter)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ /* auto site survey per 60sec */
+ if (pmlmepriv->scan_interval > 0) {
+ pmlmepriv->scan_interval--;
+ if (pmlmepriv->scan_interval == 0) {
+ DBG_88E("%s\n", __func__);
+ rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+ pmlmepriv->scan_interval = SCAN_INTERVAL;/* 30*2 sec = 60sec */
+ }
+ }
+}
+
+void rtw_dynamic_check_timer_handlder(struct adapter *adapter)
+{
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ struct registry_priv *pregistrypriv = &adapter->registrypriv;
+
+ if (!adapter)
+ return;
+ if (!adapter->hw_init_completed)
+ return;
+ if ((adapter->bDriverStopped) || (adapter->bSurpriseRemoved))
+ return;
+ if (adapter->net_closed)
+ return;
+ rtw_dynamic_chk_wk_cmd(adapter);
+
+ if (pregistrypriv->wifi_spec == 1) {
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+#endif
+ {
+ /* auto site survey */
+ rtw_auto_scan_handler(adapter);
+ }
+ }
+
+ rcu_read_lock();
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+ if (rcu_dereference(adapter->pnetdev->rx_handler_data) &&
+#else
+ if (rcu_dereference(adapter->pnetdev->br_port) &&
+#endif
+ (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == true)) {
+ /* expire NAT2.5 entry */
+ nat25_db_expire(adapter);
+
+ if (adapter->pppoe_connection_in_progress > 0) {
+ adapter->pppoe_connection_in_progress--;
+ }
+
+ /* due to rtw_dynamic_check_timer_handlder() is called every 2 seconds */
+ if (adapter->pppoe_connection_in_progress > 0) {
+ adapter->pppoe_connection_in_progress--;
+ }
+ }
+
+ rcu_read_unlock();
+}
+
+#define RTW_SCAN_RESULT_EXPIRE 2000
+
+/*
+* Select a new join candidate from the original @param candidate and @param competitor
+* @return true: candidate is updated
+* @return false: candidate is not updated
+*/
+static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
+ , struct wlan_network **candidate, struct wlan_network *competitor)
+{
+ int updated = false;
+ struct adapter *adapter = container_of(pmlmepriv, struct adapter, mlmepriv);
+
+ /* check bssid, if needed */
+ if (pmlmepriv->assoc_by_bssid) {
+ if (memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN))
+ goto exit;
+ }
+
+ /* check ssid, if needed */
+ if (pmlmepriv->assoc_ssid.Ssid && pmlmepriv->assoc_ssid.SsidLength) {
+ if (competitor->network.Ssid.SsidLength != pmlmepriv->assoc_ssid.SsidLength ||
+ memcmp(competitor->network.Ssid.Ssid, pmlmepriv->assoc_ssid.Ssid, pmlmepriv->assoc_ssid.SsidLength))
+ goto exit;
+ }
+
+ if (rtw_is_desired_network(adapter, competitor) == false)
+ goto exit;
+
+ if(rtw_to_roaming(adapter) > 0) {
+ if (rtw_get_passing_time_ms((u32)competitor->last_scanned) >= RTW_SCAN_RESULT_EXPIRE ||
+ is_same_ess(&competitor->network, &pmlmepriv->cur_network.network) == false)
+ goto exit;
+ }
+
+ if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) {
+ *candidate = competitor;
+ updated = true;
+ }
+ if (updated) {
+ DBG_88E("[by_bssid:%u][assoc_ssid:%s]new candidate: %s(%pM rssi:%d\n",
+ pmlmepriv->assoc_by_bssid,
+ pmlmepriv->assoc_ssid.Ssid,
+ (*candidate)->network.Ssid.Ssid,
+ (*candidate)->network.MacAddress,
+ (int)(*candidate)->network.Rssi);
+ DBG_88E("[to_roaming:%u]\n",rtw_to_roaming(adapter));
+ }
+
+exit:
+ return updated;
+}
+
+/*
+Calling context:
+The caller of the sub-routine will be in critical section...
+The caller must hold the following spinlock
+pmlmepriv->lock
+*/
+
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
+{
+ int ret;
+ struct list_head *phead;
+ struct adapter *adapter;
+ struct __queue *queue = &(pmlmepriv->scanned_queue);
+ struct wlan_network *pnetwork = NULL;
+ struct wlan_network *candidate = NULL;
+ u8 supp_ant_div = false;
+
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+ phead = get_list_head(queue);
+ adapter = (struct adapter *)pmlmepriv->nic_hdl;
+ pmlmepriv->pscanned = phead->next;
+ while (phead != pmlmepriv->pscanned) {
+ pnetwork = container_of(pmlmepriv->pscanned, struct wlan_network, list);
+ if (pnetwork == NULL) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork==NULL)\n", __func__));
+ ret = _FAIL;
+ goto exit;
+ }
+ pmlmepriv->pscanned = pmlmepriv->pscanned->next;
+ rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+ }
+ if (candidate == NULL) {
+ DBG_88E("%s: return _FAIL(candidate==NULL)\n", __func__);
+ ret = _FAIL;
+ goto exit;
+ } else {
+ DBG_88E("%s: candidate: %s(%pM ch:%u)\n", __func__,
+ candidate->network.Ssid.Ssid, candidate->network.MacAddress,
+ candidate->network.Configuration.DSConfig);
+ }
+
+ /* check for situation of _FW_LINKED */
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+ DBG_88E("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
+
+ rtw_disassoc_cmd(adapter, 0, true);
+ rtw_indicate_disconnect(adapter);
+ rtw_free_assoc_resources(adapter, 0);
+ }
+
+ rtw_hal_get_def_var(adapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(supp_ant_div));
+ if (supp_ant_div) {
+ u8 cur_ant;
+ rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(cur_ant));
+ DBG_88E("#### Opt_Ant_(%s), cur_Ant(%s)\n",
+ (2 == candidate->network.PhyInfo.Optimum_antenna) ? "A" : "B",
+ (2 == cur_ant) ? "A" : "B"
+ );
+ }
+
+ ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+ return ret;
+}
+
+int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv)
+{
+ struct cmd_obj *pcmd;
+ struct setauth_parm *psetauthparm;
+ struct cmd_priv *pcmdpriv = &(adapter->cmdpriv);
+ int res = _SUCCESS;
+
+ pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd == NULL) {
+ res = _FAIL; /* try again */
+ goto exit;
+ }
+
+ psetauthparm = (struct setauth_parm *)rtw_zmalloc(sizeof(struct setauth_parm));
+ if (psetauthparm == NULL) {
+ kfree(pcmd);
+ res = _FAIL;
+ goto exit;
+ }
+ memset(psetauthparm, 0, sizeof(struct setauth_parm));
+ psetauthparm->mode = (unsigned char)psecuritypriv->dot11AuthAlgrthm;
+ pcmd->cmdcode = _SetAuth_CMD_;
+ pcmd->parmbuf = (unsigned char *)psetauthparm;
+ pcmd->cmdsz = (sizeof(struct setauth_parm));
+ pcmd->rsp = NULL;
+ pcmd->rspsz = 0;
+ INIT_LIST_HEAD(&pcmd->list);
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("after enqueue set_auth_cmd, auth_mode=%x\n",
+ psecuritypriv->dot11AuthAlgrthm));
+ res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+
+ return res;
+}
+
+int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, int keyid, u8 set_tx)
+{
+ u8 keylen;
+ struct cmd_obj *pcmd;
+ struct setkey_parm *psetkeyparm;
+ struct cmd_priv *pcmdpriv = &(adapter->cmdpriv);
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+ int res = _SUCCESS;
+
+ pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd == NULL) {
+ res = _FAIL; /* try again */
+ goto exit;
+ }
+ psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
+ if (psetkeyparm == NULL) {
+ kfree(pcmd);
+ res = _FAIL;
+ goto exit;
+ }
+
+ memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+ if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+ psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("\n rtw_set_key: psetkeyparm->algorithm=(unsigned char)psecuritypriv->dot118021XGrpPrivacy=%d\n",
+ psetkeyparm->algorithm));
+ } else {
+ psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("\n rtw_set_key: psetkeyparm->algorithm=(u8)psecuritypriv->dot11PrivacyAlgrthm=%d\n",
+ psetkeyparm->algorithm));
+ }
+ psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+ psetkeyparm->set_tx = set_tx;
+ pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
+ DBG_88E("==> rtw_set_key algorithm(%x), keyid(%x), key_mask(%x)\n",
+ psetkeyparm->algorithm, psetkeyparm->keyid, pmlmepriv->key_mask);
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("\n rtw_set_key: psetkeyparm->algorithm=%d psetkeyparm->keyid=(u8)keyid=%d\n",
+ psetkeyparm->algorithm, keyid));
+
+ switch (psetkeyparm->algorithm) {
+ case _WEP40_:
+ keylen = 5;
+ memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+ break;
+ case _WEP104_:
+ keylen = 13;
+ memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+ break;
+ case _TKIP_:
+ keylen = 16;
+ memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+ psetkeyparm->grpkey = 1;
+ break;
+ case _AES_:
+ keylen = 16;
+ memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+ psetkeyparm->grpkey = 1;
+ break;
+ default:
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm=%x (must be 1 or 2 or 4 or 5)\n",
+ psecuritypriv->dot11PrivacyAlgrthm));
+ res = _FAIL;
+ goto exit;
+ }
+ pcmd->cmdcode = _SetKey_CMD_;
+ pcmd->parmbuf = (u8 *)psetkeyparm;
+ pcmd->cmdsz = (sizeof(struct setkey_parm));
+ pcmd->rsp = NULL;
+ pcmd->rspsz = 0;
+ INIT_LIST_HEAD(&pcmd->list);
+ res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+
+ return res;
+}
+
+/* adjust IEs for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+ unsigned int ielength = 0;
+ unsigned int i, j;
+
+ i = 12; /* after the fixed IE */
+ while (i < in_len) {
+ ielength = initial_out_len;
+
+ if (in_ie[i] == 0xDD && in_ie[i+2] == 0x00 && in_ie[i+3] == 0x50 && in_ie[i+4] == 0xF2 && in_ie[i+5] == 0x02 && i+5 < in_len) {
+ /* WMM element ID and OUI */
+ /* Append WMM IE to the last index of out_ie */
+
+ for (j = i; j < i + 9; j++) {
+ out_ie[ielength] = in_ie[j];
+ ielength++;
+ }
+ out_ie[initial_out_len + 1] = 0x07;
+ out_ie[initial_out_len + 6] = 0x00;
+ out_ie[initial_out_len + 8] = 0x00;
+ break;
+ }
+ i += (in_ie[i+1]+2); /* to the next IE element */
+ }
+ return ielength;
+}
+
+/* */
+/* Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.) */
+/* Added by Annie, 2006-05-07. */
+/* */
+/* Search by BSSID, */
+/* Return Value: */
+/* -1 :if there is no pre-auth key in the table */
+/* >= 0 :if there is pre-auth key, and return the entry id */
+/* */
+/* */
+
+static int SecIsInPMKIDList(struct adapter *Adapter, u8 *bssid)
+{
+ struct security_priv *psecuritypriv = &Adapter->securitypriv;
+ int i = 0;
+
+ do {
+ if ((psecuritypriv->PMKIDList[i].bUsed) &&
+ (!memcmp(psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN))) {
+ break;
+ } else {
+ i++;
+ /* continue; */
+ }
+
+ } while (i < NUM_PMKID_CACHE);
+
+ if (i == NUM_PMKID_CACHE) {
+ i = -1;/* Could not find. */
+ } else {
+ /* There is one Pre-Authentication Key for the specific BSSID. */
+ }
+ return i;
+}
+
+/* */
+/* Check the RSN IE length */
+/* If the RSN IE length <= 20, the RSN IE didn't include the PMKID information */
+/* 0-11th element in the array are the fixed IE */
+/* 12th element in the array is the IE */
+/* 13th element in the array is the IE length */
+/* */
+
+static int rtw_append_pmkid(struct adapter *Adapter, int iEntry, u8 *ie, uint ie_len)
+{
+ struct security_priv *psecuritypriv = &Adapter->securitypriv;
+
+ if (ie[13] <= 20) {
+ /* The RSN IE didn't include the PMK ID, append the PMK information */
+ ie[ie_len] = 1;
+ ie_len++;
+ ie[ie_len] = 0; /* PMKID count = 0x0100 */
+ ie_len++;
+ memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+
+ ie_len += 16;
+ ie[13] += 18;/* PMKID length = 2+16 */
+ }
+ return ie_len;
+}
+
+int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+ u8 authmode;
+ uint ielength;
+ int iEntry;
+
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+ uint ndisauthmode = psecuritypriv->ndisauthtype;
+ uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+ ("+rtw_restruct_sec_ie: ndisauthmode=%d ndissecuritytype=%d\n",
+ ndisauthmode, ndissecuritytype));
+
+ /* copy fixed ie only */
+ memcpy(out_ie, in_ie, 12);
+ ielength = 12;
+ if ((ndisauthmode == Ndis802_11AuthModeWPA) ||
+ (ndisauthmode == Ndis802_11AuthModeWPAPSK))
+ authmode = _WPA_IE_ID_;
+ if ((ndisauthmode == Ndis802_11AuthModeWPA2) ||
+ (ndisauthmode == Ndis802_11AuthModeWPA2PSK))
+ authmode = _WPA2_IE_ID_;
+
+ if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+ memcpy(out_ie+ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len);
+
+ ielength += psecuritypriv->wps_ie_len;
+ } else if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) {
+ /* copy RSN or SSN */
+ memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1]+2);
+ ielength += psecuritypriv->supplicant_ie[1]+2;
+ rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+ }
+
+ iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+ if (iEntry < 0) {
+ return ielength;
+ } else {
+ if (authmode == _WPA2_IE_ID_)
+ ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+ }
+
+ return ielength;
+}
+
+void rtw_init_registrypriv_dev_network(struct adapter *adapter)
+{
+ struct registry_priv *pregistrypriv = &adapter->registrypriv;
+ struct eeprom_priv *peepriv = &adapter->eeprompriv;
+ struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+ u8 *myhwaddr = myid(peepriv);
+
+ memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+ memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid));
+
+ pdev_network->Configuration.Length = sizeof(struct ndis_802_11_config);
+ pdev_network->Configuration.BeaconPeriod = 100;
+ pdev_network->Configuration.FHConfig.Length = 0;
+ pdev_network->Configuration.FHConfig.HopPattern = 0;
+ pdev_network->Configuration.FHConfig.HopSet = 0;
+ pdev_network->Configuration.FHConfig.DwellTime = 0;
+
+}
+
+void rtw_update_registrypriv_dev_network(struct adapter *adapter)
+{
+ int sz = 0;
+ struct registry_priv *pregistrypriv = &adapter->registrypriv;
+ struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+ struct wlan_network *cur_network = &adapter->mlmepriv.cur_network;
+
+ pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0); /* adhoc no 802.1x */
+
+ pdev_network->Rssi = 0;
+
+ switch (pregistrypriv->wireless_mode) {
+ case WIRELESS_11B:
+ pdev_network->NetworkTypeInUse = (Ndis802_11DS);
+ break;
+ case WIRELESS_11G:
+ case WIRELESS_11BG:
+ case WIRELESS_11_24N:
+ case WIRELESS_11G_24N:
+ case WIRELESS_11BG_24N:
+ pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+ break;
+ case WIRELESS_11A:
+ case WIRELESS_11A_5N:
+ pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+ break;
+ case WIRELESS_11ABGN:
+ if (pregistrypriv->channel > 14)
+ pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+ else
+ pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+ break;
+ default:
+ /* TODO */
+ break;
+ }
+
+ pdev_network->Configuration.DSConfig = (pregistrypriv->channel);
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("pregistrypriv->channel=%d, pdev_network->Configuration.DSConfig=0x%x\n",
+ pregistrypriv->channel, pdev_network->Configuration.DSConfig));
+
+ if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+ pdev_network->Configuration.ATIMWindow = (0);
+
+ pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+ /* 1. Supported rates */
+ /* 2. IE */
+
+ sz = rtw_generate_ie(pregistrypriv);
+ pdev_network->IELength = sz;
+ pdev_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+
+ /* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */
+ /* pdev_network->IELength = cpu_to_le32(sz); */
+
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter)
+{
+
+}
+
+/* the function is at passive_level */
+void rtw_joinbss_reset(struct adapter *padapter)
+{
+ u8 threshold;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+ /* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+ pmlmepriv->num_FortyMHzIntolerant = 0;
+
+ pmlmepriv->num_sta_no_ht = 0;
+
+ phtpriv->ampdu_enable = false;/* reset to disabled */
+
+ /* TH = 1 => means that invalidate usb rx aggregation */
+ /* TH = 0 => means that validate usb rx aggregation, use init value. */
+ if (phtpriv->ht_option) {
+ if (padapter->registrypriv.wifi_spec == 1)
+ threshold = 1;
+ else
+ threshold = 0;
+ rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+ } else {
+ threshold = 1;
+ rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+ }
+}
+
+/* the function is >= passive_level */
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len)
+{
+ u32 ielen, out_len;
+ enum ht_cap_ampdu_factor max_rx_ampdu_factor;
+ unsigned char *p;
+ struct ieee80211_ht_cap ht_capie;
+ unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+ u32 rx_packet_offset, max_recvbuf_sz;
+
+ phtpriv->ht_option = false;
+
+ p = rtw_get_ie(in_ie+12, _HT_CAPABILITY_IE_, &ielen, in_len-12);
+
+ if (p && ielen > 0) {
+ if (pqospriv->qos_option == 0) {
+ out_len = *pout_len;
+ rtw_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_,
+ _WMM_IE_Length_, WMM_IE, pout_len);
+
+ pqospriv->qos_option = 1;
+ }
+
+ out_len = *pout_len;
+
+ memset(&ht_capie, 0, sizeof(struct ieee80211_ht_cap));
+
+ ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH |
+ IEEE80211_HT_CAP_SGI_20 |
+ IEEE80211_HT_CAP_SGI_40 |
+ IEEE80211_HT_CAP_TX_STBC |
+ IEEE80211_HT_CAP_DSSSCCK40);
+
+ rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+ rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+
+ /*
+ AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+ AMPDU_para [4:2]:Min MPDU Start Spacing
+ */
+
+ rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+ ht_capie.ampdu_params_info = (max_rx_ampdu_factor&0x03);
+
+ if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+ ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));
+ else
+ ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);
+
+ rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
+ sizeof(struct ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+ phtpriv->ht_option = true;
+
+ p = rtw_get_ie(in_ie+12, _HT_ADD_INFO_IE_, &ielen, in_len-12);
+ if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+ out_len = *pout_len;
+ rtw_set_ie(out_ie+out_len, _HT_ADD_INFO_IE_, ielen, p+2 , pout_len);
+ }
+ }
+ return phtpriv->ht_option;
+}
+
+/* the function is > passive_level (in critical_section) */
+void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len)
+{
+ u8 *p, max_ampdu_sz;
+ int len;
+ struct ieee80211_ht_cap *pht_capie;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (!phtpriv->ht_option)
+ return;
+
+ if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable))
+ return;
+
+ DBG_88E("+rtw_update_ht_cap()\n");
+
+ /* maybe needs check if ap supports rx ampdu. */
+ if ((!phtpriv->ampdu_enable) && (pregistrypriv->ampdu_enable == 1)) {
+ if (pregistrypriv->wifi_spec == 1)
+ phtpriv->ampdu_enable = false;
+ else
+ phtpriv->ampdu_enable = true;
+ } else if (pregistrypriv->ampdu_enable == 2) {
+ phtpriv->ampdu_enable = true;
+ }
+
+ /* check Max Rx A-MPDU Size */
+ len = 0;
+ p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fixed_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fixed_ie));
+ if (p && len > 0) {
+ pht_capie = (struct ieee80211_ht_cap *)(p+2);
+ max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+ max_ampdu_sz = 1 << (max_ampdu_sz+3); /* max_ampdu_sz (kbytes); */
+ phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+ }
+ len = 0;
+ p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fixed_ie), _HT_ADD_INFO_IE_, &len, ie_len-sizeof(struct ndis_802_11_fixed_ie));
+
+ /* update cur_bwmode & cur_ch_offset */
+ if ((pregistrypriv->cbw40_enable) &&
+ (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & BIT(1)) &&
+ (pmlmeinfo->HT_info.infos[0] & BIT(2))) {
+ int i;
+ u8 rf_type;
+
+ padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+ /* update the MCS rates */
+ for (i = 0; i < 16; i++) {
+ if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+ pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+ else
+ pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+ }
+ /* switch to the 40M Hz mode according to the AP */
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+ switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) {
+ case HT_EXTCHNL_OFFSET_UPPER:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+ break;
+ case HT_EXTCHNL_OFFSET_LOWER:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+ break;
+ default:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ break;
+ }
+ }
+
+ /* Config SM Power Save setting */
+ pmlmeinfo->SM_PS = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & 0x0C) >> 2;
+ if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+ DBG_88E("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+ /* Config current HT Protection mode. */
+ pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ u8 issued;
+ int priority;
+ struct sta_info *psta = NULL;
+ struct ht_priv *phtpriv;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ s32 bmcst = IS_MCAST(pattrib->ra);
+
+ if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100))
+ return;
+
+ priority = pattrib->priority;
+
+ if (pattrib->psta)
+ psta = pattrib->psta;
+ else
+ psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+ if (psta == NULL)
+ return;
+
+ phtpriv = &psta->htpriv;
+
+ if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+ issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+ issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+ if (0 == issued) {
+ DBG_88E("rtw_issue_addbareq_cmd, p=%d\n", priority);
+ psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+ rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra);
+ }
+ }
+}
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ spin_lock_bh(&pmlmepriv->lock);
+ _rtw_roaming(padapter, tgt_network);
+ spin_unlock_bh(&pmlmepriv->lock);
+}
+void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ int do_join_r;
+
+ struct wlan_network *pnetwork;
+
+ if (tgt_network != NULL)
+ pnetwork = tgt_network;
+ else
+ pnetwork = &pmlmepriv->cur_network;
+
+ if (0 < rtw_to_roaming(padapter)) {
+ DBG_88E("roaming from %s(%pM length:%d\n",
+ pnetwork->network.Ssid.Ssid, pnetwork->network.MacAddress,
+ pnetwork->network.Ssid.SsidLength);
+ memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid, sizeof(struct ndis_802_11_ssid));
+
+ pmlmepriv->assoc_by_bssid = false;
+
+ while (1) {
+ do_join_r = rtw_do_join(padapter);
+ if (_SUCCESS == do_join_r) {
+ break;
+ } else {
+ DBG_88E("roaming do_join return %d\n", do_join_r);
+ pmlmepriv->to_roaming--;
+
+ if (0 < pmlmepriv->to_roaming) {
+ continue;
+ } else {
+ DBG_88E("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
+ rtw_indicate_disconnect(padapter);
+ break;
+ }
+ }
+ }
+ }
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mlme_ext.c b/drivers/staging/r8188eu/core/rtw_mlme_ext.c
new file mode 100644
index 000000000000..6c420d5238a8
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mlme_ext.c
@@ -0,0 +1,8407 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MLME_EXT_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <rtw_mlme_ext.h>
+#include <wlan_bssdef.h>
+#include <mlme_osdep.h>
+#include <recv_osdep.h>
+
+static struct mlme_handler mlme_sta_tbl[] = {
+ {WIFI_ASSOCREQ, "OnAssocReq", &OnAssocReq},
+ {WIFI_ASSOCRSP, "OnAssocRsp", &OnAssocRsp},
+ {WIFI_REASSOCREQ, "OnReAssocReq", &OnAssocReq},
+ {WIFI_REASSOCRSP, "OnReAssocRsp", &OnAssocRsp},
+ {WIFI_PROBEREQ, "OnProbeReq", &OnProbeReq},
+ {WIFI_PROBERSP, "OnProbeRsp", &OnProbeRsp},
+
+ /*----------------------------------------------------------
+ below 2 are reserved
+ -----------------------------------------------------------*/
+ {0, "DoReserved", &DoReserved},
+ {0, "DoReserved", &DoReserved},
+ {WIFI_BEACON, "OnBeacon", &OnBeacon},
+ {WIFI_ATIM, "OnATIM", &OnAtim},
+ {WIFI_DISASSOC, "OnDisassoc", &OnDisassoc},
+ {WIFI_AUTH, "OnAuth", &OnAuthClient},
+ {WIFI_DEAUTH, "OnDeAuth", &OnDeAuth},
+ {WIFI_ACTION, "OnAction", &OnAction},
+};
+
+static struct action_handler OnAction_tbl[] = {
+ {RTW_WLAN_CATEGORY_SPECTRUM_MGMT, "ACTION_SPECTRUM_MGMT", on_action_spct},
+ {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
+ {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
+ {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+ {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+ {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+ {RTW_WLAN_CATEGORY_FT, "ACTION_FT", &DoReserved},
+ {RTW_WLAN_CATEGORY_HT, "ACTION_HT", &OnAction_ht},
+ {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
+ {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
+ {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
+};
+
+static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+/**************************************************
+OUI definitions for the vendor specific IE
+***************************************************/
+unsigned char RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+unsigned char P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};
+unsigned char WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A};
+
+unsigned char WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+unsigned char WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+
+extern unsigned char REALTEK_96B_IE[];
+
+/********************************************************
+MCS rate definitions
+*********************************************************/
+unsigned char MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+unsigned char MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+static struct rt_channel_plan_2g RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
+ {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13}, /* 0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
+ {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13}, /* 0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
+ {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11}, /* 0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
+ {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14}, /* 0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
+ {{10, 11, 12, 13}, 4}, /* 0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
+ {{}, 0}, /* 0x05, RT_CHANNEL_DOMAIN_2G_NULL */
+};
+
+static struct rt_channel_plan_map RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+ /* 0x00 ~ 0x1F , Old Define ===== */
+ {0x02}, /* 0x00, RT_CHANNEL_DOMAIN_FCC */
+ {0x02}, /* 0x01, RT_CHANNEL_DOMAIN_IC */
+ {0x01}, /* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+ {0x01}, /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+ {0x01}, /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+ {0x03}, /* 0x05, RT_CHANNEL_DOMAIN_MKK */
+ {0x03}, /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+ {0x01}, /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+ {0x03}, /* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+ {0x03}, /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+ {0x00}, /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+ {0x02}, /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+ {0x01}, /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+ {0x02}, /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+ {0x02}, /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+ {0x02}, /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+ {0x01}, /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+ {0x02}, /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+ {0x01}, /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+ {0x00}, /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
+ {0x02}, /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+ {0x00}, /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+ {0x00}, /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+ {0x03}, /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+ {0x05}, /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+ {0x02}, /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+ {0x00}, /* 0x1A, */
+ {0x00}, /* 0x1B, */
+ {0x00}, /* 0x1C, */
+ {0x00}, /* 0x1D, */
+ {0x00}, /* 0x1E, */
+ {0x05}, /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
+ /* 0x20 ~ 0x7F , New Define ===== */
+ {0x00}, /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+ {0x01}, /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+ {0x02}, /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+ {0x03}, /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+ {0x04}, /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+ {0x02}, /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+ {0x00}, /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+ {0x03}, /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+ {0x00}, /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+ {0x00}, /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+ {0x00}, /* 0x2A, */
+ {0x00}, /* 0x2B, */
+ {0x00}, /* 0x2C, */
+ {0x00}, /* 0x2D, */
+ {0x00}, /* 0x2E, */
+ {0x00}, /* 0x2F, */
+ {0x00}, /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+ {0x00}, /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+ {0x00}, /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+ {0x00}, /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+ {0x02}, /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+ {0x00}, /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+ {0x00}, /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+ {0x03}, /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+ {0x03}, /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+ {0x02}, /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+ {0x00}, /* 0x3A, */
+ {0x00}, /* 0x3B, */
+ {0x00}, /* 0x3C, */
+ {0x00}, /* 0x3D, */
+ {0x00}, /* 0x3E, */
+ {0x00}, /* 0x3F, */
+ {0x02}, /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+ {0x03}, /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
+};
+
+static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03}; /* use the conbination for max channel numbers */
+
+/*
+ * Search the @param channel_num in given @param channel_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ *
+ * return the index of channel_num in channel_set, -1 if not found
+ */
+int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
+{
+ int i;
+ for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+ if (ch == ch_set[i].ChannelNum)
+ break;
+ }
+
+ if (i >= ch_set[i].ChannelNum)
+ return -1;
+ return i;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+ return _SUCCESS;
+}
+
+static void init_mlme_ext_priv_value(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ unsigned char mixed_datarate[NumRates] = {
+ _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+ _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
+ _48M_RATE_, _54M_RATE_, 0xff
+ };
+ unsigned char mixed_basicrate[NumRates] = {
+ _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+ _12M_RATE_, _24M_RATE_, 0xff,
+ };
+
+ ATOMIC_SET(&pmlmeext->event_seq, 0);
+ pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
+
+ pmlmeext->cur_channel = padapter->registrypriv.channel;
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ pmlmeext->retry = 0;
+
+ pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+ memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+ memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+ pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+
+ pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+ pmlmeext->sitesurvey_res.channel_idx = 0;
+ pmlmeext->sitesurvey_res.bss_cnt = 0;
+ pmlmeext->scan_abort = false;
+
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ pmlmeinfo->reauth_count = 0;
+ pmlmeinfo->reassoc_count = 0;
+ pmlmeinfo->link_count = 0;
+ pmlmeinfo->auth_seq = 0;
+ pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+ pmlmeinfo->key_index = 0;
+ pmlmeinfo->iv = 0;
+
+ pmlmeinfo->enc_algo = _NO_PRIVACY_;
+ pmlmeinfo->authModeToggle = 0;
+
+ memset(pmlmeinfo->chg_txt, 0, 128);
+
+ pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+ pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+ pmlmeinfo->dialogToken = 0;
+
+ pmlmeext->action_public_rxseq = 0xffff;
+ pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(struct rt_channel_info *channel_set,
+ u8 chanset_size,
+ u8 chan) {
+ int i;
+
+ for (i = 0; i < chanset_size; i++) {
+ if (channel_set[i].ChannelNum == chan)
+ return 1;
+ }
+ return 0;
+}
+
+static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
+ u8 chanset_size,
+ struct p2p_channels *channel_list) {
+ struct p2p_oper_class_map op_class[] = {
+ { IEEE80211G, 81, 1, 13, 1, BW20 },
+ { IEEE80211G, 82, 14, 14, 1, BW20 },
+ { -1, 0, 0, 0, 0, BW20 }
+ };
+
+ int cla, op;
+
+ cla = 0;
+
+ for (op = 0; op_class[op].op_class; op++) {
+ u8 ch;
+ struct p2p_oper_class_map *o = &op_class[op];
+ struct p2p_reg_class *reg = NULL;
+
+ for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+ if (!has_channel(channel_set, chanset_size, ch)) {
+ continue;
+ }
+
+ if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+ continue;
+
+ if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
+ ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+ continue;
+
+ if (reg == NULL) {
+ reg = &channel_list->reg_class[cla];
+ cla++;
+ reg->reg_class = o->op_class;
+ reg->channels = 0;
+ }
+ reg->channel[reg->channels] = ch;
+ reg->channels++;
+ }
+ }
+ channel_list->reg_classes = cla;
+}
+
+static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
+{
+ u8 index, chanset_size = 0;
+ u8 b2_4GBand = false;
+ u8 Index2G = 0;
+
+ memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
+
+ if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+ DBG_88E("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
+ return chanset_size;
+ }
+
+ if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
+ b2_4GBand = true;
+ if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+ Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+ else
+ Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+ }
+
+ if (b2_4GBand) {
+ for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
+ channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+ if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
+ (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)) {
+ if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+ channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+ else if ((channel_set[chanset_size].ChannelNum >= 12 && channel_set[chanset_size].ChannelNum <= 14))
+ channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+ } else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
+ RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {/* channel 12~13, passive scan */
+ if (channel_set[chanset_size].ChannelNum <= 11)
+ channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+ else
+ channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+ } else {
+ channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+ }
+
+ chanset_size++;
+ }
+ }
+ return chanset_size;
+}
+
+int init_mlme_ext_priv(struct adapter *padapter)
+{
+ int res = _SUCCESS;
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ pmlmeext->padapter = padapter;
+
+ init_mlme_ext_priv_value(padapter);
+ pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+
+ init_mlme_ext_timer(padapter);
+
+#ifdef CONFIG_88EU_AP_MODE
+ init_mlme_ap_info(padapter);
+#endif
+
+ pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
+ init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+ pmlmeext->chan_scan_time = SURVEY_TO;
+ pmlmeext->mlmeext_init = true;
+
+ pmlmeext->active_keep_alive_check = true;
+
+ return res;
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+ struct adapter *padapter = pmlmeext->padapter;
+
+ if (!padapter)
+ return;
+
+ if (padapter->bDriverStopped) {
+ _cancel_timer_ex(&pmlmeext->survey_timer);
+ _cancel_timer_ex(&pmlmeext->link_timer);
+ /* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
+ }
+}
+
+static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, struct recv_frame *precv_frame)
+{
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+ u8 *pframe = precv_frame->rx_data;
+
+ if (ptable->func) {
+ /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+ if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+ memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+ return;
+ ptable->func(padapter, precv_frame);
+ }
+}
+
+void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ int index;
+ struct mlme_handler *ptable;
+#ifdef CONFIG_88EU_AP_MODE
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* CONFIG_88EU_AP_MODE */
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+ u8 *pframe = precv_frame->rx_data;
+ struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
+ GetFrameType(pframe), GetFrameSubType(pframe)));
+
+ if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
+ return;
+ }
+
+ /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+ if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+ memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+ return;
+
+ ptable = mlme_sta_tbl;
+
+ index = GetFrameSubType(pframe) >> 4;
+
+ if (index > 13) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type=%d\n", index));
+ return;
+ }
+ ptable += index;
+
+ if (psta != NULL) {
+ if (GetRetry(pframe)) {
+ if (precv_frame->attrib.seq_num == psta->RxMgmtFrameSeqNum) {
+ /* drop the duplicate management frame */
+ DBG_88E("Drop duplicate management frame with seq_num=%d.\n", precv_frame->attrib.seq_num);
+ return;
+ }
+ }
+ psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
+ }
+
+#ifdef CONFIG_88EU_AP_MODE
+ switch (GetFrameSubType(pframe)) {
+ case WIFI_AUTH:
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+ ptable->func = &OnAuth;
+ else
+ ptable->func = &OnAuthClient;
+ /* fall through */
+ case WIFI_ASSOCREQ:
+ case WIFI_REASSOCREQ:
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ break;
+ case WIFI_PROBEREQ:
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ else
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ break;
+ case WIFI_BEACON:
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ break;
+ case WIFI_ACTION:
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ break;
+ default:
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+ rtw_hostapd_mlme_rx(padapter, precv_frame);
+ break;
+ }
+#else
+ _mgt_dispatcher(padapter, ptable, precv_frame);
+#endif
+}
+
+#ifdef CONFIG_88EU_P2P
+static u32 p2p_listen_state_process(struct adapter *padapter, unsigned char *da)
+{
+ bool response = true;
+
+ /* do nothing if the device name is empty */
+ if (!padapter->wdinfo.device_name_len)
+ response = false;
+
+ if (response)
+ issue_probersp_p2p(padapter, da);
+
+ return _SUCCESS;
+}
+#endif /* CONFIG_88EU_P2P */
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ unsigned int ielen;
+ unsigned char *p;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
+ u8 *pframe = precv_frame->rx_data;
+ uint len = precv_frame->len;
+ u8 is_valid_p2p_probereq = false;
+
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 wifi_test_chk_rate = 1;
+
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
+ !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
+ !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
+ !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
+ !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
+ /* mcs_rate = 0 -> CCK 1M rate */
+ /* mcs_rate = 1 -> CCK 2M rate */
+ /* mcs_rate = 2 -> CCK 5.5M rate */
+ /* mcs_rate = 3 -> CCK 11M rate */
+ /* In the P2P mode, the driver should not support the CCK rate */
+
+ /* Commented by Kurt 2012/10/16 */
+ /* IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
+ if (wifi_test_chk_rate == 1) {
+ is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
+ if (is_valid_p2p_probereq) {
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+ /* FIXME */
+ report_survey_event(padapter, precv_frame);
+ p2p_listen_state_process(padapter, get_sa(pframe));
+
+ return _SUCCESS;
+ }
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+ goto _continue;
+ }
+ }
+ }
+
+_continue:
+#endif /* CONFIG_88EU_P2P */
+
+ if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+ return _SUCCESS;
+
+ if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+ !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
+ return _SUCCESS;
+
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+ len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+ /* check (wildcard) SSID */
+ if (p != NULL) {
+ if (is_valid_p2p_probereq)
+ goto _issue_probersp;
+
+ if ((ielen != 0 && memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
+ (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
+ return _SUCCESS;
+
+_issue_probersp:
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+ (pmlmepriv->cur_network.join_res ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
+ issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+ }
+ return _SUCCESS;
+}
+
+unsigned int OnProbeRsp(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+ u8 *pframe = precv_frame->rx_data;
+#endif
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+ if (pwdinfo->tx_prov_disc_info.benable) {
+ if (!memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+ pwdinfo->tx_prov_disc_info.benable = false;
+ issue_p2p_provision_request(padapter,
+ pwdinfo->tx_prov_disc_info.ssid.Ssid,
+ pwdinfo->tx_prov_disc_info.ssid.SsidLength,
+ pwdinfo->tx_prov_disc_info.peerDevAddr);
+ } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ pwdinfo->tx_prov_disc_info.benable = false;
+ issue_p2p_provision_request(padapter, NULL, 0,
+ pwdinfo->tx_prov_disc_info.peerDevAddr);
+ }
+ }
+ }
+ return _SUCCESS;
+ } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+ if (pwdinfo->nego_req_info.benable) {
+ DBG_88E("[%s] P2P State is GONEGO ING!\n", __func__);
+ if (!memcmp(pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+ pwdinfo->nego_req_info.benable = false;
+ issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr);
+ }
+ }
+ } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
+ if (pwdinfo->invitereq_info.benable) {
+ DBG_88E("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
+ if (!memcmp(pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
+ pwdinfo->invitereq_info.benable = false;
+ issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr);
+ }
+ }
+ }
+#endif
+
+ if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+ report_survey_event(padapter, precv_frame);
+ return _SUCCESS;
+ }
+
+ return _SUCCESS;
+}
+
+unsigned int OnBeacon(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ int cam_idx;
+ struct sta_info *psta;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 *pframe = precv_frame->rx_data;
+ uint len = precv_frame->len;
+ struct wlan_bssid_ex *pbss;
+ int ret = _SUCCESS;
+
+ if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+ report_survey_event(padapter, precv_frame);
+ return _SUCCESS;
+ }
+
+ if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) {
+ if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+ /* we should update current network before auth, or some IE is wrong */
+ pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
+ if (pbss) {
+ if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+ update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
+ rtw_get_bcn_info(&(pmlmepriv->cur_network));
+ }
+ kfree(pbss);
+ }
+
+ /* check the vendor of the assoc AP */
+ pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ /* update TSF Value */
+ update_TSF(pmlmeext, pframe, len);
+
+ /* start auth */
+ start_clnt_auth(padapter);
+
+ return _SUCCESS;
+ }
+
+ if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+ if (psta != NULL) {
+ ret = rtw_check_bcn_info(padapter, pframe, len);
+ if (!ret) {
+ DBG_88E_LEVEL(_drv_info_, "ap has changed, disconnect now\n ");
+ receive_disconnect(padapter, pmlmeinfo->network.MacAddress, 0);
+ return _SUCCESS;
+ }
+ /* update WMM, ERP in the beacon */
+ /* todo: the timer is used instead of the number of the beacon received */
+ if ((sta_rx_pkts(psta) & 0xf) == 0)
+ update_beacon_info(padapter, pframe, len, psta);
+ process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+ }
+ } else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+ if (psta != NULL) {
+ /* update WMM, ERP in the beacon */
+ /* todo: the timer is used instead of the number of the beacon received */
+ if ((sta_rx_pkts(psta) & 0xf) == 0)
+ update_beacon_info(padapter, pframe, len, psta);
+ } else {
+ /* allocate a new CAM entry for IBSS station */
+ cam_idx = allocate_fw_sta_entry(padapter);
+ if (cam_idx == NUM_STA)
+ goto _END_ONBEACON_;
+
+ /* get supported rate */
+ if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
+ pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+ goto _END_ONBEACON_;
+ }
+
+ /* update TSF Value */
+ update_TSF(pmlmeext, pframe, len);
+
+ /* report sta add event */
+ report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+ }
+ }
+ }
+
+_END_ONBEACON_:
+
+ return _SUCCESS;
+}
+
+unsigned int OnAuth(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ unsigned int auth_mode, ie_len;
+ u16 seq;
+ unsigned char *sa, *p;
+ u16 algorithm;
+ int status;
+ static struct sta_info stat;
+ struct sta_info *pstat = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 *pframe = precv_frame->rx_data;
+ uint len = precv_frame->len;
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ return _FAIL;
+
+ DBG_88E("+OnAuth\n");
+
+ sa = GetAddr2Ptr(pframe);
+
+ auth_mode = psecuritypriv->dot11AuthAlgrthm;
+ seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
+ algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
+
+ DBG_88E("auth alg=%x, seq=%X\n", algorithm, seq);
+
+ if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+ psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+ auth_mode = 0;
+
+ if ((algorithm > 0 && auth_mode == 0) || /* rx a shared-key auth but shared not enabled */
+ (algorithm == 0 && auth_mode == 1)) { /* rx a open-system auth but shared-key is enabled */
+ DBG_88E("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
+ algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+
+ status = _STATS_NO_SUPP_ALG_;
+
+ goto auth_fail;
+ }
+
+ if (!rtw_access_ctrl(padapter, sa)) {
+ status = _STATS_UNABLE_HANDLE_STA_;
+ goto auth_fail;
+ }
+
+ pstat = rtw_get_stainfo(pstapriv, sa);
+ if (pstat == NULL) {
+ /* allocate a new one */
+ DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
+ pstat = rtw_alloc_stainfo(pstapriv, sa);
+ if (pstat == NULL) {
+ DBG_88E(" Exceed the upper limit of supported clients...\n");
+ status = _STATS_UNABLE_HANDLE_STA_;
+ goto auth_fail;
+ }
+
+ pstat->state = WIFI_FW_AUTH_NULL;
+ pstat->auth_seq = 0;
+ } else {
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ if (!list_empty(&pstat->asoc_list)) {
+ list_del_init(&pstat->asoc_list);
+ pstapriv->asoc_list_cnt--;
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ if (seq == 1) {
+ /* TODO: STA re_auth and auth timeout */
+ }
+ }
+
+ spin_lock_bh(&pstapriv->auth_list_lock);
+ if (list_empty(&pstat->auth_list)) {
+ list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+ pstapriv->auth_list_cnt++;
+ }
+ spin_unlock_bh(&pstapriv->auth_list_lock);
+
+ if (pstat->auth_seq == 0)
+ pstat->expire_to = pstapriv->auth_to;
+
+ if ((pstat->auth_seq + 1) != seq) {
+ DBG_88E("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+ seq, pstat->auth_seq+1);
+ status = _STATS_OUT_OF_AUTH_SEQ_;
+ goto auth_fail;
+ }
+
+ if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
+ if (seq == 1) {
+ pstat->state &= ~WIFI_FW_AUTH_NULL;
+ pstat->state |= WIFI_FW_AUTH_SUCCESS;
+ pstat->expire_to = pstapriv->assoc_to;
+ pstat->authalg = algorithm;
+ } else {
+ DBG_88E("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+ seq, pstat->auth_seq+1);
+ status = _STATS_OUT_OF_AUTH_SEQ_;
+ goto auth_fail;
+ }
+ } else { /* shared system or auto authentication */
+ if (seq == 1) {
+ /* prepare for the challenging txt... */
+
+ pstat->state &= ~WIFI_FW_AUTH_NULL;
+ pstat->state |= WIFI_FW_AUTH_STATE;
+ pstat->authalg = algorithm;
+ pstat->auth_seq = 2;
+ } else if (seq == 3) {
+ /* checking for challenging txt... */
+ DBG_88E("checking for challenging txt...\n");
+
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
+ len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+ if ((p == NULL) || (ie_len <= 0)) {
+ DBG_88E("auth rejected because challenge failure!(1)\n");
+ status = _STATS_CHALLENGE_FAIL_;
+ goto auth_fail;
+ }
+
+ if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+ pstat->state &= (~WIFI_FW_AUTH_STATE);
+ pstat->state |= WIFI_FW_AUTH_SUCCESS;
+ /* challenging txt is correct... */
+ pstat->expire_to = pstapriv->assoc_to;
+ } else {
+ DBG_88E("auth rejected because challenge failure!\n");
+ status = _STATS_CHALLENGE_FAIL_;
+ goto auth_fail;
+ }
+ } else {
+ DBG_88E("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+ seq, pstat->auth_seq+1);
+ status = _STATS_OUT_OF_AUTH_SEQ_;
+ goto auth_fail;
+ }
+ }
+
+ /* Now, we are going to issue_auth... */
+ pstat->auth_seq = seq + 1;
+
+#ifdef CONFIG_88EU_AP_MODE
+ issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+#endif
+
+ if (pstat->state & WIFI_FW_AUTH_SUCCESS)
+ pstat->auth_seq = 0;
+
+ return _SUCCESS;
+
+auth_fail:
+
+ if (pstat)
+ rtw_free_stainfo(padapter , pstat);
+
+ pstat = &stat;
+ memset((char *)pstat, '\0', sizeof(stat));
+ pstat->auth_seq = 2;
+ memcpy(pstat->hwaddr, sa, 6);
+
+#ifdef CONFIG_88EU_AP_MODE
+ issue_auth(padapter, pstat, (unsigned short)status);
+#endif
+
+#endif
+ return _FAIL;
+}
+
+unsigned int OnAuthClient(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ unsigned int seq, len, status, offset;
+ unsigned char *p;
+ unsigned int go2asoc = 0;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 *pframe = precv_frame->rx_data;
+ uint pkt_len = precv_frame->len;
+
+ DBG_88E("%s\n", __func__);
+
+ /* check A1 matches or not */
+ if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+ return _SUCCESS;
+
+ if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+ return _SUCCESS;
+
+ offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+ seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
+ status = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+ if (status != 0) {
+ DBG_88E("clnt auth fail, status: %d\n", status);
+ if (status == 13) { /* pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+ if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+ pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+ else
+ pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+ }
+
+ set_link_timer(pmlmeext, 1);
+ goto authclnt_fail;
+ }
+
+ if (seq == 2) {
+ if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+ /* legendary shared system */
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+ pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+ if (p == NULL)
+ goto authclnt_fail;
+
+ memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+ pmlmeinfo->auth_seq = 3;
+ issue_auth(padapter, NULL, 0);
+ set_link_timer(pmlmeext, REAUTH_TO);
+
+ return _SUCCESS;
+ } else {
+ /* open system */
+ go2asoc = 1;
+ }
+ } else if (seq == 4) {
+ if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+ go2asoc = 1;
+ else
+ goto authclnt_fail;
+ } else {
+ /* this is also illegal */
+ goto authclnt_fail;
+ }
+
+ if (go2asoc) {
+ DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
+ start_clnt_assoc(padapter);
+ return _SUCCESS;
+ }
+authclnt_fail:
+ return _FAIL;
+}
+
+unsigned int OnAssocReq(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ u16 capab_info;
+ struct rtw_ieee802_11_elems elems;
+ struct sta_info *pstat;
+ unsigned char reassoc, *p, *pos, *wpa_ie;
+ unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+ int i, ie_len, wpa_ie_len, left;
+ unsigned char supportRate[16];
+ int supportRateNum;
+ unsigned short status = _STATS_SUCCESSFUL_;
+ unsigned short frame_type, ie_offset = 0;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 *pframe = precv_frame->rx_data;
+ uint pkt_len = precv_frame->len;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 p2p_status_code = P2P_STATUS_SUCCESS;
+ u8 *p2pie;
+ u32 p2pielen = 0;
+#endif /* CONFIG_88EU_P2P */
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ return _FAIL;
+
+ frame_type = GetFrameSubType(pframe);
+ if (frame_type == WIFI_ASSOCREQ) {
+ reassoc = 0;
+ ie_offset = _ASOCREQ_IE_OFFSET_;
+ } else { /* WIFI_REASSOCREQ */
+ reassoc = 1;
+ ie_offset = _REASOCREQ_IE_OFFSET_;
+ }
+
+ if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+ DBG_88E("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
+ "\n", reassoc, (unsigned long)pkt_len);
+ return _FAIL;
+ }
+
+ pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+ if (pstat == (struct sta_info *)NULL) {
+ status = _RSON_CLS2_;
+ goto asoc_class2_error;
+ }
+
+ capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);
+
+ left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+ pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+ DBG_88E("%s\n", __func__);
+
+ /* check if this stat has been successfully authenticated/assocated */
+ if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+ if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+ status = _RSON_CLS2_;
+ goto asoc_class2_error;
+ } else {
+ pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+ pstat->state |= WIFI_FW_ASSOC_STATE;
+ }
+ } else {
+ pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+ pstat->state |= WIFI_FW_ASSOC_STATE;
+ }
+ pstat->capability = capab_info;
+ /* now parse all ieee802_11 ie to point to elems */
+ if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+ !elems.ssid) {
+ DBG_88E("STA %pM sent invalid association request\n",
+ pstat->hwaddr);
+ status = _STATS_FAILURE_;
+ goto OnAssocReqFail;
+ }
+
+ /* now we should check all the fields... */
+ /* checking SSID */
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+ pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+ if (p == NULL)
+ status = _STATS_FAILURE_;
+
+ if (ie_len == 0) { /* broadcast ssid, however it is not allowed in assocreq */
+ status = _STATS_FAILURE_;
+ } else {
+ /* check if ssid match */
+ if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+ status = _STATS_FAILURE_;
+
+ if (ie_len != cur->Ssid.SsidLength)
+ status = _STATS_FAILURE_;
+ }
+
+ if (_STATS_SUCCESSFUL_ != status)
+ goto OnAssocReqFail;
+
+ /* check if the supported rate is ok */
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+ if (p == NULL) {
+ DBG_88E("Rx a sta assoc-req which supported rate is empty!\n");
+ /* use our own rate set as statoin used */
+ /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+ /* supportRateNum = AP_BSSRATE_LEN; */
+
+ status = _STATS_FAILURE_;
+ goto OnAssocReqFail;
+ } else {
+ memcpy(supportRate, p+2, ie_len);
+ supportRateNum = ie_len;
+
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
+ pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+ if (p != NULL) {
+ if (supportRateNum <= sizeof(supportRate)) {
+ memcpy(supportRate+supportRateNum, p+2, ie_len);
+ supportRateNum += ie_len;
+ }
+ }
+ }
+
+ /* todo: mask supportRate between AP & STA -> move to update raid */
+ /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
+
+ /* update station supportRate */
+ pstat->bssratelen = supportRateNum;
+ memcpy(pstat->bssrateset, supportRate, supportRateNum);
+ UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+ /* check RSN/WPA/WPS */
+ pstat->dot8021xalg = 0;
+ pstat->wpa_psk = 0;
+ pstat->wpa_group_cipher = 0;
+ pstat->wpa2_group_cipher = 0;
+ pstat->wpa_pairwise_cipher = 0;
+ pstat->wpa2_pairwise_cipher = 0;
+ memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+ if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+ int group_cipher = 0, pairwise_cipher = 0;
+
+ wpa_ie = elems.rsn_ie;
+ wpa_ie_len = elems.rsn_ie_len;
+
+ if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+ pstat->dot8021xalg = 1;/* psk, todo:802.1x */
+ pstat->wpa_psk |= BIT(1);
+
+ pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
+ pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
+
+ if (!pstat->wpa2_group_cipher)
+ status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+ if (!pstat->wpa2_pairwise_cipher)
+ status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+ } else {
+ status = WLAN_STATUS_INVALID_IE;
+ }
+ } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+ int group_cipher = 0, pairwise_cipher = 0;
+
+ wpa_ie = elems.wpa_ie;
+ wpa_ie_len = elems.wpa_ie_len;
+
+ if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+ pstat->dot8021xalg = 1;/* psk, todo:802.1x */
+ pstat->wpa_psk |= BIT(0);
+
+ pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
+ pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
+
+ if (!pstat->wpa_group_cipher)
+ status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+ if (!pstat->wpa_pairwise_cipher)
+ status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+ } else {
+ status = WLAN_STATUS_INVALID_IE;
+ }
+ } else {
+ wpa_ie = NULL;
+ wpa_ie_len = 0;
+ }
+
+ if (_STATS_SUCCESSFUL_ != status)
+ goto OnAssocReqFail;
+
+ pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+ if (wpa_ie == NULL) {
+ if (elems.wps_ie) {
+ DBG_88E("STA included WPS IE in "
+ "(Re)Association Request - assume WPS is "
+ "used\n");
+ pstat->flags |= WLAN_STA_WPS;
+ /* wpabuf_free(sta->wps_ie); */
+ /* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+ /* elems.wps_ie_len - 4); */
+ } else {
+ DBG_88E("STA did not include WPA/RSN IE "
+ "in (Re)Association Request - possible WPS "
+ "use\n");
+ pstat->flags |= WLAN_STA_MAYBE_WPS;
+ }
+
+ /* AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+ /* that the selected registrar of AP is _FLASE */
+ if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+ if (pmlmepriv->wps_beacon_ie) {
+ u8 selected_registrar = 0;
+
+ rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
+
+ if (!selected_registrar) {
+ DBG_88E("selected_registrar is false , or AP is not ready to do WPS\n");
+
+ status = _STATS_UNABLE_HANDLE_STA_;
+
+ goto OnAssocReqFail;
+ }
+ }
+ }
+ } else {
+ int copy_len;
+
+ if (psecuritypriv->wpa_psk == 0) {
+ DBG_88E("STA %pM: WPA/RSN IE in association "
+ "request, but AP don't support WPA/RSN\n", pstat->hwaddr);
+
+ status = WLAN_STATUS_INVALID_IE;
+
+ goto OnAssocReqFail;
+ }
+
+ if (elems.wps_ie) {
+ DBG_88E("STA included WPS IE in "
+ "(Re)Association Request - WPS is "
+ "used\n");
+ pstat->flags |= WLAN_STA_WPS;
+ copy_len = 0;
+ } else {
+ copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len+2);
+ }
+ if (copy_len > 0)
+ memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
+ }
+ /* check if there is WMM IE & support WWM-PS */
+ pstat->flags &= ~WLAN_STA_WME;
+ pstat->qos_option = 0;
+ pstat->qos_info = 0;
+ pstat->has_legacy_ac = true;
+ pstat->uapsd_vo = 0;
+ pstat->uapsd_vi = 0;
+ pstat->uapsd_be = 0;
+ pstat->uapsd_bk = 0;
+ if (pmlmepriv->qospriv.qos_option) {
+ p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
+ for (;;) {
+ p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+ if (p != NULL) {
+ if (!memcmp(p+2, WMM_IE, 6)) {
+ pstat->flags |= WLAN_STA_WME;
+
+ pstat->qos_option = 1;
+ pstat->qos_info = *(p+8);
+
+ pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+
+ if ((pstat->qos_info&0xf) != 0xf)
+ pstat->has_legacy_ac = true;
+ else
+ pstat->has_legacy_ac = false;
+
+ if (pstat->qos_info&0xf) {
+ if (pstat->qos_info&BIT(0))
+ pstat->uapsd_vo = BIT(0)|BIT(1);
+ else
+ pstat->uapsd_vo = 0;
+
+ if (pstat->qos_info&BIT(1))
+ pstat->uapsd_vi = BIT(0)|BIT(1);
+ else
+ pstat->uapsd_vi = 0;
+
+ if (pstat->qos_info&BIT(2))
+ pstat->uapsd_bk = BIT(0)|BIT(1);
+ else
+ pstat->uapsd_bk = 0;
+
+ if (pstat->qos_info&BIT(3))
+ pstat->uapsd_be = BIT(0)|BIT(1);
+ else
+ pstat->uapsd_be = 0;
+ }
+ break;
+ }
+ } else {
+ break;
+ }
+ p = p + ie_len + 2;
+ }
+ }
+
+ /* save HT capabilities in the sta object */
+ memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
+ if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap)) {
+ pstat->flags |= WLAN_STA_HT;
+
+ pstat->flags |= WLAN_STA_WME;
+
+ memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
+ } else {
+ pstat->flags &= ~WLAN_STA_HT;
+ }
+ if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags&WLAN_STA_HT)) {
+ status = _STATS_FAILURE_;
+ goto OnAssocReqFail;
+ }
+
+ if ((pstat->flags & WLAN_STA_HT) &&
+ ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+ (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
+ DBG_88E("HT: %pM tried to "
+ "use TKIP with HT association\n", pstat->hwaddr);
+
+ /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
+ /* goto OnAssocReqFail; */
+ }
+
+ pstat->flags |= WLAN_STA_NONERP;
+ for (i = 0; i < pstat->bssratelen; i++) {
+ if ((pstat->bssrateset[i] & 0x7f) > 22) {
+ pstat->flags &= ~WLAN_STA_NONERP;
+ break;
+ }
+ }
+
+ if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+ pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+ else
+ pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+ if (status != _STATS_SUCCESSFUL_)
+ goto OnAssocReqFail;
+
+#ifdef CONFIG_88EU_P2P
+ pstat->is_p2p_device = false;
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen);
+ if (p2pie) {
+ pstat->is_p2p_device = true;
+ p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
+ if (p2p_status_code > 0) {
+ pstat->p2p_status_code = p2p_status_code;
+ status = _STATS_CAP_FAIL_;
+ goto OnAssocReqFail;
+ }
+ }
+ }
+ pstat->p2p_status_code = p2p_status_code;
+#endif /* CONFIG_88EU_P2P */
+
+ /* TODO: identify_proprietary_vendor_ie(); */
+ /* Realtek proprietary IE */
+ /* identify if this is Broadcom sta */
+ /* identify if this is ralink sta */
+ /* Customer proprietary IE */
+
+ /* get a unique AID */
+ if (pstat->aid > 0) {
+ DBG_88E(" old AID %d\n", pstat->aid);
+ } else {
+ for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+ if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
+ break;
+
+ /* if (pstat->aid > NUM_STA) { */
+ if (pstat->aid > pstapriv->max_num_sta) {
+ pstat->aid = 0;
+
+ DBG_88E(" no room for more AIDs\n");
+
+ status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+ goto OnAssocReqFail;
+ } else {
+ pstapriv->sta_aid[pstat->aid - 1] = pstat;
+ DBG_88E("allocate new AID=(%d)\n", pstat->aid);
+ }
+ }
+
+ pstat->state &= (~WIFI_FW_ASSOC_STATE);
+ pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+
+ spin_lock_bh(&pstapriv->auth_list_lock);
+ if (!list_empty(&pstat->auth_list)) {
+ list_del_init(&pstat->auth_list);
+ pstapriv->auth_list_cnt--;
+ }
+ spin_unlock_bh(&pstapriv->auth_list_lock);
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ if (list_empty(&pstat->asoc_list)) {
+ pstat->expire_to = pstapriv->expire_to;
+ list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+ pstapriv->asoc_list_cnt++;
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ /* now the station is qualified to join our BSS... */
+ if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
+#ifdef CONFIG_88EU_AP_MODE
+ /* 1 bss_cap_update & sta_info_update */
+ bss_cap_update_on_sta_join(padapter, pstat);
+ sta_info_update(padapter, pstat);
+
+ /* issue assoc rsp before notify station join event. */
+ if (frame_type == WIFI_ASSOCREQ)
+ issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+ else
+ issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+ /* 2 - report to upper layer */
+ DBG_88E("indicate_sta_join_event to upper layer - hostapd\n");
+ rtw_indicate_sta_assoc_event(padapter, pstat);
+
+ /* 3-(1) report sta add event */
+ report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+#endif
+ }
+
+ return _SUCCESS;
+
+asoc_class2_error:
+
+#ifdef CONFIG_88EU_AP_MODE
+ issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+#endif
+
+ return _FAIL;
+
+OnAssocReqFail:
+
+#ifdef CONFIG_88EU_AP_MODE
+ pstat->aid = 0;
+ if (frame_type == WIFI_ASSOCREQ)
+ issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+ else
+ issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+#endif
+
+#endif /* CONFIG_88EU_AP_MODE */
+
+ return _FAIL;
+}
+
+unsigned int OnAssocRsp(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ uint i;
+ int res;
+ unsigned short status;
+ struct ndis_802_11_var_ie *pIE;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ /* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
+ u8 *pframe = precv_frame->rx_data;
+ uint pkt_len = precv_frame->len;
+
+ DBG_88E("%s\n", __func__);
+
+ /* check A1 matches or not */
+ if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+ return _SUCCESS;
+
+ if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+ return _SUCCESS;
+
+ if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+ return _SUCCESS;
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ /* status */
+ status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+ if (status > 0) {
+ DBG_88E("assoc reject, status code: %d\n", status);
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ res = -4;
+ goto report_assoc_result;
+ }
+
+ /* get capabilities */
+ pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+ /* set slot time */
+ pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+ /* AID */
+ pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
+ res = pmlmeinfo->aid;
+
+ /* following are moved to join event callback function */
+ /* to handle HT, WMM, rate adaptive, update MAC reg */
+ /* for not to handle the synchronous IO in the tasklet */
+ for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
+ pIE = (struct ndis_802_11_var_ie *)(pframe + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if (!memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */
+ WMM_param_handler(padapter, pIE);
+ break;
+ case _HT_CAPABILITY_IE_: /* HT caps */
+ HT_caps_handler(padapter, pIE);
+ break;
+ case _HT_EXTRA_INFO_IE_: /* HT info */
+ HT_info_handler(padapter, pIE);
+ break;
+ case _ERPINFO_IE_:
+ ERP_IE_handler(padapter, pIE);
+ default:
+ break;
+ }
+
+ i += (pIE->Length + 2);
+ }
+
+ pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+ pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+ /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+ UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+ if (res > 0) {
+ rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+ } else {
+ rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+ }
+
+ report_join_res(padapter, res);
+
+ return _SUCCESS;
+}
+
+unsigned int OnDeAuth(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ unsigned short reason;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 *pframe = precv_frame->rx_data;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ /* check A3 */
+ if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+ return _SUCCESS;
+
+#ifdef CONFIG_88EU_P2P
+ if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+ _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+ DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ struct sta_info *psta;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ DBG_88E_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
+ reason, GetAddr2Ptr(pframe));
+
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+ if (psta) {
+ u8 updated = 0;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ if (!list_empty(&psta->asoc_list)) {
+ list_del_init(&psta->asoc_list);
+ pstapriv->asoc_list_cnt--;
+ updated = ap_free_sta(padapter, psta, false, reason);
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ associated_clients_update(padapter, updated);
+ }
+
+ return _SUCCESS;
+ } else
+#endif
+ {
+ int ignore_received_deauth = 0;
+
+ /* Before sending the auth frame to start the STA/GC mode connection with AP/GO,
+ * we will send the deauth first.
+ * However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth.
+ * Added the following code to avoid this case.
+ */
+ if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) ||
+ (pmlmeinfo->state & WIFI_FW_ASSOC_STATE )) {
+ if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA) {
+ ignore_received_deauth = 1;
+ } else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
+ // TODO: 802.11r
+ ignore_received_deauth = 1;
+ }
+ }
+
+ DBG_88E_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
+ reason, GetAddr3Ptr(pframe), ignore_received_deauth);
+
+ if (!ignore_received_deauth)
+ receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+ }
+ pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+ return _SUCCESS;
+}
+
+unsigned int OnDisassoc(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ u16 reason;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 *pframe = precv_frame->rx_data;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ /* check A3 */
+ if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+ return _SUCCESS;
+
+#ifdef CONFIG_88EU_P2P
+ if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+ _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+ DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ struct sta_info *psta;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+ reason, GetAddr2Ptr(pframe));
+
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+ if (psta) {
+ u8 updated = 0;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ if (!list_empty(&psta->asoc_list)) {
+ list_del_init(&psta->asoc_list);
+ pstapriv->asoc_list_cnt--;
+ updated = ap_free_sta(padapter, psta, false, reason);
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ associated_clients_update(padapter, updated);
+ }
+
+ return _SUCCESS;
+ } else
+#endif
+ {
+ DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+ reason, GetAddr3Ptr(pframe));
+
+ receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+ }
+ pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+ return _SUCCESS;
+}
+
+unsigned int OnAtim(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ DBG_88E("%s\n", __func__);
+ return _SUCCESS;
+}
+
+unsigned int on_action_spct(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ unsigned int ret = _FAIL;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 *pframe = precv_frame->rx_data;
+ u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+ u8 category;
+ u8 action;
+
+ DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+ if (!psta)
+ goto exit;
+
+ category = frame_body[0];
+ if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+ goto exit;
+
+ action = frame_body[1];
+ switch (action) {
+ case RTW_WLAN_ACTION_SPCT_MSR_REQ:
+ case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+ case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+ case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+ break;
+ case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+ break;
+ default:
+ break;
+ }
+
+exit:
+ return ret;
+}
+
+unsigned int OnAction_qos(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ return _SUCCESS;
+}
+
+unsigned int OnAction_dls(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ return _SUCCESS;
+}
+
+unsigned int OnAction_back(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ u8 *addr;
+ struct sta_info *psta = NULL;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ unsigned char *frame_body;
+ unsigned char category, action;
+ unsigned short tid, status, reason_code = 0;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 *pframe = precv_frame->rx_data;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ /* check RA matches or not */
+ if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+ return _SUCCESS;
+
+ DBG_88E("%s\n", __func__);
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+ return _SUCCESS;
+
+ addr = GetAddr2Ptr(pframe);
+ psta = rtw_get_stainfo(pstapriv, addr);
+
+ if (psta == NULL)
+ return _SUCCESS;
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ category = frame_body[0];
+ if (category == RTW_WLAN_CATEGORY_BACK) { /* representing Block Ack */
+ if (!pmlmeinfo->HT_enable)
+ return _SUCCESS;
+ action = frame_body[1];
+ DBG_88E("%s, action=%d\n", __func__, action);
+ switch (action) {
+ case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
+ memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+ process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
+
+ if (pmlmeinfo->bAcceptAddbaReq)
+ issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
+ else
+ issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
+ break;
+ case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+ status = get_unaligned_le16(&frame_body[3]);
+ tid = ((frame_body[5] >> 2) & 0x7);
+ if (status == 0) { /* successful */
+ DBG_88E("agg_enable for TID=%d\n", tid);
+ psta->htpriv.agg_enable_bitmap |= 1 << tid;
+ psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+ } else {
+ psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+ }
+ break;
+ case RTW_WLAN_ACTION_DELBA: /* DELBA */
+ if ((frame_body[3] & BIT(3)) == 0) {
+ psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+ psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+ reason_code = get_unaligned_le16(&frame_body[4]);
+ } else if ((frame_body[3] & BIT(3)) == BIT(3)) {
+ tid = (frame_body[3] >> 4) & 0x0F;
+ preorder_ctrl = &psta->recvreorder_ctrl[tid];
+ preorder_ctrl->enable = false;
+ preorder_ctrl->indicate_seq = 0xffff;
+ }
+ DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
+ /* todo: how to notify the host while receiving DELETE BA */
+ break;
+ default:
+ break;
+ }
+ }
+ return _SUCCESS;
+}
+
+#ifdef CONFIG_88EU_P2P
+
+static int get_reg_classes_full_count(struct p2p_channels *channel_list)
+{
+ int cnt = 0;
+ int i;
+
+ for (i = 0; i < channel_list->reg_classes; i++) {
+ cnt += channel_list->reg_class[i].channels;
+ }
+
+ return cnt;
+}
+
+void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_GO_NEGO_REQ;
+ u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+ u8 wpsielen = 0, p2pielen = 0;
+ u16 len_channellist_attr = 0;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ DBG_88E("[%s] In\n", __func__);
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pwdinfo->negotiation_dialog_token = 1; /* Initialize the dialog value */
+ pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
+
+ /* WPS Section */
+ wpsielen = 0;
+ /* WPS OUI */
+ *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* WPS version */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */
+
+ /* Device Password ID */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+
+ if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+ else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+ else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+
+ wpsielen += 2;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ /* P2P IE Section. */
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20110306 */
+ /* According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. Group Owner Intent */
+ /* 3. Configuration Timeout */
+ /* 4. Listen Channel */
+ /* 5. Extended Listen Timing */
+ /* 6. Intended P2P Interface Address */
+ /* 7. Channel List */
+ /* 8. P2P Device Info */
+ /* 9. Operating Channel */
+
+ /* P2P Capability */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+ else
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+ /* Group Owner Intent */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Todo the tie breaker bit. */
+ p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
+
+ /* Configuration Timeout */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */
+
+ /* Listen Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51; /* Copy from SD7 */
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->listen_channel; /* listening channel number */
+
+ /* Extended Listen Timing ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Availability Period */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ /* Availability Interval */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ /* Intended P2P Interface Address */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Channel List */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+ /* Length: */
+ /* Country String(3) */
+ /* + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+ /* + number of channels in all classes */
+ len_channellist_attr = 3
+ + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
+ + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Channel Entry List */
+
+ {
+ int i, j;
+ for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+ /* Operating Class */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+ /* Number of Channels */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+ /* Channel List */
+ for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+ }
+ }
+ }
+
+ /* Device Info */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ /* Operating Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51;
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+static void issue_p2p_GO_response(struct adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_GO_NEGO_RESP;
+ u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+ u8 p2pielen = 0;
+ uint wpsielen = 0;
+ u16 wps_devicepassword_id = 0x0000;
+ __be16 be_tmp;
+ uint wps_devicepassword_id_len = 0;
+ u16 len_channellist_attr = 0;
+
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ DBG_88E("[%s] In, result=%d\n", __func__, result);
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pwdinfo->negotiation_dialog_token = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */
+ pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+ /* Commented by Albert 20110328 */
+ /* Try to get the device password ID from the WPS IE of group negotiation request frame */
+ /* WiFi Direct test plan 5.1.15 */
+ rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
+ rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
+ wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+ memset(wpsie, 0x00, 255);
+ wpsielen = 0;
+
+ /* WPS Section */
+ wpsielen = 0;
+ /* WPS OUI */
+ *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* WPS version */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */
+
+ /* Device Password ID */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+ else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+ else
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+ wpsielen += 2;
+
+ /* Commented by Kurt 20120113 */
+ /* If some device wants to do p2p handshake without sending prov_disc_req */
+ /* We have to get peer_req_cm from here. */
+ if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
+ if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+ else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+ else
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+ }
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ /* P2P IE Section. */
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20100908 */
+ /* According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */
+ /* 1. Status */
+ /* 2. P2P Capability */
+ /* 3. Group Owner Intent */
+ /* 4. Configuration Timeout */
+ /* 5. Operating Channel */
+ /* 6. Intended P2P Interface Address */
+ /* 7. Channel List */
+ /* 8. Device Info */
+ /* 9. Group ID (Only GO) */
+
+ /* ToDo: */
+
+ /* P2P Status */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = result;
+
+ /* P2P Capability */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+ /* Commented by Albert 2011/03/08 */
+ /* According to the P2P specification */
+ /* if the sending device will be client, the P2P Capability should be reserved of group negotiation response frame */
+ p2pie[p2pielen++] = 0;
+ } else {
+ /* Be group owner or meet the error case */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+ }
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported) {
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+ } else {
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+ }
+
+ /* Group Owner Intent */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ if (pwdinfo->peer_intent & 0x01) {
+ /* Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
+ p2pie[p2pielen++] = (pwdinfo->intent << 1);
+ } else {
+ /* Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
+ p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
+ }
+
+ /* Configuration Timeout */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */
+
+ /* Operating Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51;
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */
+
+ /* Intended P2P Interface Address */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Channel List */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+ /* Country String(3) */
+ /* + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+ /* + number of channels in all classes */
+ len_channellist_attr = 3
+ + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+ + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Channel Entry List */
+
+ {
+ int i, j;
+ for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+ /* Operating Class */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+ /* Number of Channels */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+ /* Channel List */
+ for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+ }
+ }
+ }
+
+ /* Device Info */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* Group ID Attribute */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+ p2pielen += 2;
+
+ /* Value: */
+ /* p2P Device Address */
+ memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* SSID */
+ memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+ p2pielen += pwdinfo->nego_ssidlen;
+ }
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+ return;
+}
+
+static void issue_p2p_GO_confirm(struct adapter *padapter, u8 *raddr, u8 result)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_GO_NEGO_CONF;
+ u8 p2pie[255] = { 0x00 };
+ u8 p2pielen = 0;
+
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ DBG_88E("[%s] In\n", __func__);
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+ /* P2P IE Section. */
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20110306 */
+ /* According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */
+ /* 1. Status */
+ /* 2. P2P Capability */
+ /* 3. Operating Channel */
+ /* 4. Channel List */
+ /* 5. Group ID (if this WiFi is GO) */
+
+ /* P2P Status */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = result;
+
+ /* P2P Capability */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+ else
+ p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+ /* Operating Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51;
+ p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
+ } else {
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51;
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->operating_channel; /* Use the listen channel as the operating channel */
+ }
+
+ /* Channel List */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(pwdinfo->channel_list_attr_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len);
+ p2pielen += pwdinfo->channel_list_attr_len;
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* Group ID Attribute */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+ p2pielen += 2;
+
+ /* Value: */
+ /* p2P Device Address */
+ memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* SSID */
+ memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+ p2pielen += pwdinfo->nego_ssidlen;
+ }
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+ pattrib->last_txcmdsz = pattrib->pktlen;
+ dump_mgntframe(padapter, pmgntframe);
+ return;
+}
+
+void issue_p2p_invitation_request(struct adapter *padapter, u8 *raddr)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_INVIT_REQ;
+ u8 p2pie[255] = { 0x00 };
+ u8 p2pielen = 0;
+ u8 dialogToken = 3;
+ u16 len_channellist_attr = 0;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, raddr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ /* P2P IE Section. */
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20101011 */
+ /* According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
+ /* 1. Configuration Timeout */
+ /* 2. Invitation Flags */
+ /* 3. Operating Channel (Only GO) */
+ /* 4. P2P Group BSSID (Should be included if I am the GO) */
+ /* 5. Channel List */
+ /* 6. P2P Group ID */
+ /* 7. P2P Device Info */
+
+ /* Configuration Timeout */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */
+
+ /* Invitation Flags */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
+
+ /* Operating Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51;
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch; /* operating channel number */
+
+ if (!memcmp(myid(&padapter->eeprompriv), pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) {
+ /* P2P Group BSSID */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address for GO */
+ memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+ }
+
+ /* Channel List */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+ /* Length: */
+ /* Country String(3) */
+ /* + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+ /* + number of channels in all classes */
+ len_channellist_attr = 3
+ + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+ + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Channel Entry List */
+ {
+ int i, j;
+ for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+ /* Operating Class */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+ /* Number of Channels */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+ /* Channel List */
+ for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+ }
+ }
+ }
+
+ /* P2P Group ID */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address for GO */
+ memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* SSID */
+ memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen);
+ p2pielen += pwdinfo->invitereq_info.ssidlen;
+
+ /* Device Info */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_INVIT_RESP;
+ u8 p2pie[255] = { 0x00 };
+ u8 p2pielen = 0;
+ u16 len_channellist_attr = 0;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, raddr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ /* P2P IE Section. */
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20101005 */
+ /* According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */
+ /* 1. Status */
+ /* 2. Configuration Timeout */
+ /* 3. Operating Channel (Only GO) */
+ /* 4. P2P Group BSSID (Only GO) */
+ /* 5. Channel List */
+
+ /* P2P Status */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+ p2pielen += 2;
+
+ /* Value: */
+ /* When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
+ /* Sent the event receiving the P2P Invitation Req frame to DMP UI. */
+ /* DMP had to compare the MAC address to find out the profile. */
+ /* So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
+ /* If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */
+ /* to NB to rebuild the persistent group. */
+ p2pie[p2pielen++] = status_code;
+
+ /* Configuration Timeout */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */
+ p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */
+
+ if (status_code == P2P_STATUS_SUCCESS) {
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */
+ /* In this case, the P2P Invitation response frame should carry the two more P2P attributes. */
+ /* First one is operating channel attribute. */
+ /* Second one is P2P Group BSSID attribute. */
+
+ /* Operating Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51; /* Copy from SD7 */
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */
+
+ /* P2P Group BSSID */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address for GO */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+ }
+
+ /* Channel List */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+ /* Length: */
+ /* Country String(3) */
+ /* + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
+ /* + number of channels in all classes */
+ len_channellist_attr = 3
+ + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+ + get_reg_classes_full_count(&pmlmeext->channel_list);
+
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Channel Entry List */
+ {
+ int i, j;
+ for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+ /* Operating Class */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+ /* Number of Channels */
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+ /* Channel List */
+ for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+ p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+ }
+ }
+ }
+ }
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+void issue_p2p_provision_request(struct adapter *padapter, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
+{
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ u8 dialogToken = 1;
+ u8 oui_subtype = P2P_PROVISION_DISC_REQ;
+ u8 wpsie[100] = { 0x00 };
+ u8 wpsielen = 0;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u32 p2pielen = 0;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ DBG_88E("[%s] In\n", __func__);
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ p2pielen = build_prov_disc_request_p2p_ie(pwdinfo, pframe, pssid, ussidlen, pdev_raddr);
+
+ pframe += p2pielen;
+ pattrib->pktlen += p2pielen;
+
+ wpsielen = 0;
+ /* WPS OUI */
+ *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* WPS version */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */
+
+ /* Config Method */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
+ wpsielen += 2;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+static u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo)
+{
+ u8 i, match_result = 0;
+
+ DBG_88E("[%s] peermac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+ peermacaddr[0], peermacaddr[1], peermacaddr[2], peermacaddr[3], peermacaddr[4], peermacaddr[5]);
+
+ for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
+ DBG_88E("[%s] profileinfo_mac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+ profileinfo->peermac[0], profileinfo->peermac[1], profileinfo->peermac[2], profileinfo->peermac[3], profileinfo->peermac[4], profileinfo->peermac[5]);
+ if (!memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN)) {
+ match_result = 1;
+ DBG_88E("[%s] Match!\n", __func__);
+ break;
+ }
+ }
+ return match_result;
+}
+
+void issue_probersp_p2p(struct adapter *padapter, unsigned char *da)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned char *mac;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ u16 beacon_interval = 100;
+ u16 capInfo = 0;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 wpsie[255] = { 0x00 };
+ u32 wpsielen = 0, p2pielen = 0;
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ mac = myid(&(padapter->eeprompriv));
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+ /* Use the device address for BSSID field. */
+ memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+ pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = pattrib->hdrlen;
+ pframe += pattrib->hdrlen;
+
+ /* timestamp will be inserted by hardware */
+ pframe += 8;
+ pattrib->pktlen += 8;
+
+ /* beacon interval: 2 bytes */
+ memcpy(pframe, (unsigned char *)&beacon_interval, 2);
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* capability info: 2 bytes */
+ /* ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */
+ capInfo |= cap_ShortPremble;
+ capInfo |= cap_ShortSlot;
+
+ memcpy(pframe, (unsigned char *)&capInfo, 2);
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* SSID */
+ pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
+
+ /* supported rates... */
+ /* Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+ /* DS parameter set */
+ pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
+
+ /* Todo: WPS IE */
+ /* Noted by Albert 20100907 */
+ /* According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+ wpsielen = 0;
+ /* WPS OUI */
+ *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* WPS version */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */
+
+ /* WiFi Simple Config State */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG; /* Not Configured. */
+
+ /* Response Type */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
+
+ /* UUID-E */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ wpsielen += 0x10;
+
+ /* Manufacturer */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, "Realtek", 7);
+ wpsielen += 7;
+
+ /* Model Name */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, "8188EU", 6);
+ wpsielen += 6;
+
+ /* Model Number */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = 0x31; /* character 1 */
+
+ /* Serial Number */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, "123456" , ETH_ALEN);
+ wpsielen += ETH_ALEN;
+
+ /* Primary Device Type */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+ wpsielen += 2;
+
+ /* Value: */
+ /* Category ID */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ wpsielen += 2;
+
+ /* OUI */
+ *(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ wpsielen += 2;
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+ wpsielen += 2;
+
+ /* Value: */
+ if (pwdinfo->device_name_len) {
+ memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ wpsielen += pwdinfo->device_name_len;
+ }
+
+ /* Config Method */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+ wpsielen += 2;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
+ pframe += p2pielen;
+ pattrib->pktlen += p2pielen;
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+static int _issue_probereq_p2p(struct adapter *padapter, u8 *da, int wait_ack)
+{
+ int ret = _FAIL;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned char *mac;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+ u16 wpsielen = 0, p2pielen = 0;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ mac = myid(&(padapter->eeprompriv));
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ if (da) {
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+ } else {
+ if ((pwdinfo->p2p_info.scan_op_ch_only) || (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+ /* This two flags will be set when this is only the P2P client mode. */
+ memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+ } else {
+ /* broadcast probe request frame */
+ memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+ }
+ }
+ memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+ pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+ pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
+ else
+ pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
+
+ /* Use the OFDM rate in the P2P probe request frame. (6(B), 9(B), 12(B), 24(B), 36, 48, 54) */
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+ /* WPS IE */
+ /* Noted by Albert 20110221 */
+ /* According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+ wpsielen = 0;
+ /* WPS OUI */
+ *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* WPS version */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+ wpsielen += 2;
+
+ /* Value: */
+ wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */
+
+ if (pmlmepriv->wps_probe_req_ie == NULL) {
+ /* UUID-E */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ wpsielen += 0x10;
+
+ /* Config Method */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+ wpsielen += 2;
+ }
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+ wpsielen += 2;
+
+ /* Value: */
+ memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ wpsielen += pwdinfo->device_name_len;
+
+ /* Primary Device Type */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+ wpsielen += 2;
+
+ /* Value: */
+ /* Category ID */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
+ wpsielen += 2;
+
+ /* OUI */
+ *(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+ wpsielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
+ wpsielen += 2;
+
+ /* Device Password ID */
+ /* Type: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+ wpsielen += 2;
+
+ /* Length: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); /* Registrar-specified */
+ wpsielen += 2;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20110221 */
+ /* According to the P2P Specification, the probe request frame should contain 5 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. P2P Device ID if this probe request wants to find the specific P2P device */
+ /* 3. Listen Channel */
+ /* 4. Extended Listen Timing */
+ /* 5. Operating Channel if this WiFi is working as the group owner now */
+
+ /* P2P Capability */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+ else
+ p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+ /* Listen Channel */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51; /* Copy from SD7 */
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->listen_channel; /* listen channel */
+
+ /* Extended Listen Timing */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Availability Period */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ /* Availability Interval */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* Operating Channel (if this WiFi is working as the group owner now) */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Country String */
+ p2pie[p2pielen++] = 'X';
+ p2pie[p2pielen++] = 'X';
+
+ /* The third byte should be set to 0x04. */
+ /* Described in the "Operating Channel Attribute" section. */
+ p2pie[p2pielen++] = 0x04;
+
+ /* Operating Class */
+ p2pie[p2pielen++] = 0x51; /* Copy from SD7 */
+
+ /* Channel Number */
+ p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */
+ }
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+ if (pmlmepriv->wps_probe_req_ie != NULL) {
+ /* WPS IE */
+ memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+ pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+ pframe += pmlmepriv->wps_probe_req_ie_len;
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
+
+ if (wait_ack) {
+ ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+ } else {
+ dump_mgntframe(padapter, pmgntframe);
+ ret = _SUCCESS;
+ }
+
+exit:
+ return ret;
+}
+
+inline void issue_probereq_p2p(struct adapter *adapter, u8 *da)
+{
+ _issue_probereq_p2p(adapter, da, false);
+}
+
+int issue_probereq_p2p_ex(struct adapter *adapter, u8 *da, int try_cnt, int wait_ms)
+{
+ int ret;
+ int i = 0;
+ u32 start = jiffies;
+
+ do {
+ ret = _issue_probereq_p2p(adapter, da, wait_ms > 0 ? true : false);
+
+ i++;
+
+ if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+ break;
+
+ if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+ rtw_msleep_os(wait_ms);
+ } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+ if (ret != _FAIL) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+
+ if (try_cnt && wait_ms) {
+ if (da)
+ DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(adapter), da, rtw_get_oper_ch(adapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ else
+ DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ }
+exit:
+ return ret;
+}
+
+#endif /* CONFIG_88EU_P2P */
+
+static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
+{
+ struct adapter *adapter = recv_frame->adapter;
+ struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+ u8 *frame = recv_frame->rx_data;
+ u16 seq_ctrl = ((recv_frame->attrib.seq_num&0xffff) << 4) |
+ (recv_frame->attrib.frag_num & 0xf);
+
+ if (GetRetry(frame)) {
+ if (token >= 0) {
+ if ((seq_ctrl == mlmeext->action_public_rxseq) && (token == mlmeext->action_public_dialog_token)) {
+ DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
+ FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+ return _FAIL;
+ }
+ } else {
+ if (seq_ctrl == mlmeext->action_public_rxseq) {
+ DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
+ FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
+ return _FAIL;
+ }
+ }
+ }
+
+ mlmeext->action_public_rxseq = seq_ctrl;
+
+ if (token >= 0)
+ mlmeext->action_public_dialog_token = token;
+
+ return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(struct recv_frame *precv_frame)
+{
+ u8 *pframe = precv_frame->rx_data;
+ u8 *frame_body;
+ u8 dialogToken = 0;
+#ifdef CONFIG_88EU_P2P
+ struct adapter *padapter = precv_frame->adapter;
+ uint len = precv_frame->len;
+ u8 *p2p_ie;
+ u32 p2p_ielen;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 result = P2P_STATUS_SUCCESS;
+ u8 empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+#endif /* CONFIG_88EU_P2P */
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ dialogToken = frame_body[7];
+
+ if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+ return _FAIL;
+
+#ifdef CONFIG_88EU_P2P
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+ /* Do nothing if the driver doesn't enable the P2P function. */
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+ return _SUCCESS;
+
+ len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ switch (frame_body[6]) { /* OUI Subtype */
+ case P2P_GO_NEGO_REQ:
+ DBG_88E("[%s] Got GO Nego Req Frame\n", __func__);
+ memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+ rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) {
+ /* Commented by Albert 20110526 */
+ /* In this case, this means the previous nego fail doesn't be reset yet. */
+ _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+ /* Restore the previous p2p state */
+ rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+ DBG_88E("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
+ }
+
+ /* Commented by Kurt 20110902 */
+ /* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+ rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+ /* Commented by Kurt 20120113 */
+ /* Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
+ if (!memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN))
+ memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+ result = process_p2p_group_negotation_req(pwdinfo, frame_body, len);
+ issue_p2p_GO_response(padapter, GetAddr2Ptr(pframe), frame_body, len, result);
+
+ /* Commented by Albert 20110718 */
+ /* No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
+ _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+ break;
+ case P2P_GO_NEGO_RESP:
+ DBG_88E("[%s] Got GO Nego Resp Frame\n", __func__);
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+ /* Commented by Albert 20110425 */
+ /* The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
+ _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+ pwdinfo->nego_req_info.benable = false;
+ result = process_p2p_group_negotation_resp(pwdinfo, frame_body, len);
+ issue_p2p_GO_confirm(pwdinfo->padapter, GetAddr2Ptr(pframe), result);
+ if (P2P_STATUS_SUCCESS == result) {
+ if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+ pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+ pwdinfo->p2p_info.scan_op_ch_only = 1;
+ _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+ }
+ }
+ /* Reset the dialog token for group negotiation frames. */
+ pwdinfo->negotiation_dialog_token = 1;
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+ _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+ } else {
+ DBG_88E("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
+ }
+ break;
+ case P2P_GO_NEGO_CONF:
+ DBG_88E("[%s] Got GO Nego Confirm Frame\n", __func__);
+ result = process_p2p_group_negotation_confirm(pwdinfo, frame_body, len);
+ if (P2P_STATUS_SUCCESS == result) {
+ if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+ pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+ pwdinfo->p2p_info.scan_op_ch_only = 1;
+ _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+ }
+ }
+ break;
+ case P2P_INVIT_REQ:
+ /* Added by Albert 2010/10/05 */
+ /* Received the P2P Invite Request frame. */
+
+ DBG_88E("[%s] Got invite request frame!\n", __func__);
+ p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+ if (p2p_ie) {
+ /* Parse the necessary information from the P2P Invitation Request frame. */
+ /* For example: The MAC address of sending this P2P Invitation Request frame. */
+ u32 attr_contentlen = 0;
+ u8 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ struct group_id_info group_id;
+ u8 invitation_flag = 0;
+
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
+ if (attr_contentlen) {
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
+ /* Commented by Albert 20120510 */
+ /* Copy to the pwdinfo->p2p_peer_interface_addr. */
+ /* So that the WFD UI (or Sigma) can get the peer interface address by using the following command. */
+ /* #> iwpriv wlan0 p2p_get peer_ifa */
+ /* After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
+
+ if (attr_contentlen) {
+ DBG_88E("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+ pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
+ pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
+ pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
+ }
+
+ if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT) {
+ /* Re-invoke the persistent group. */
+
+ memset(&group_id, 0x00, sizeof(struct group_id_info));
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
+ if (attr_contentlen) {
+ if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+ /* The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ status_code = P2P_STATUS_SUCCESS;
+ } else {
+ /* The p2p device sending this p2p invitation request wants to be the persistent GO. */
+ if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[0])) {
+ u8 operatingch_info[5] = { 0x00 };
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
+ if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4])) {
+ /* The operating channel is acceptable for this device. */
+ pwdinfo->rx_invitereq_info.operation_ch[0] = operatingch_info[4];
+ pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
+ _set_timer(&pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ status_code = P2P_STATUS_SUCCESS;
+ } else {
+ /* The operating channel isn't supported by this device. */
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
+ _set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
+ }
+ } else {
+ /* Commented by Albert 20121130 */
+ /* Intel will use the different P2P IE to store the operating channel information */
+ /* Workaround for Intel WiDi 3.5 */
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ status_code = P2P_STATUS_SUCCESS;
+ }
+ } else {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+ status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+ }
+ }
+ } else {
+ DBG_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+ status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ }
+ } else {
+ /* Received the invitation to join a P2P group. */
+
+ memset(&group_id, 0x00, sizeof(struct group_id_info));
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
+ if (attr_contentlen) {
+ if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+ /* In this case, the GO can't be myself. */
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+ status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ } else {
+ /* The p2p device sending this p2p invitation request wants to join an existing P2P group */
+ /* Commented by Albert 2012/06/28 */
+ /* In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
+ /* The peer device address should be the destination address for the provisioning discovery request. */
+ /* Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
+ /* The peer interface address should be the address for WPS mac address */
+ memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
+ status_code = P2P_STATUS_SUCCESS;
+ }
+ } else {
+ DBG_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+ status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ }
+ }
+ } else {
+ DBG_88E("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
+ status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ }
+
+ DBG_88E("[%s] status_code = %d\n", __func__, status_code);
+
+ pwdinfo->inviteresp_info.token = frame_body[7];
+ issue_p2p_invitation_response(padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code);
+ }
+ break;
+ case P2P_INVIT_RESP: {
+ u8 attr_content = 0x00;
+ u32 attr_contentlen = 0;
+
+ DBG_88E("[%s] Got invite response frame!\n", __func__);
+ _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+ p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+ if (p2p_ie) {
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+
+ if (attr_contentlen == 1) {
+ DBG_88E("[%s] Status = %d\n", __func__, attr_content);
+ pwdinfo->invitereq_info.benable = false;
+
+ if (attr_content == P2P_STATUS_SUCCESS) {
+ if (!memcmp(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv), ETH_ALEN)) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ }
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+ }
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+ }
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+ }
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL))
+ _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+ break;
+ }
+ case P2P_DEVDISC_REQ:
+ process_p2p_devdisc_req(pwdinfo, pframe, len);
+ break;
+ case P2P_DEVDISC_RESP:
+ process_p2p_devdisc_resp(pwdinfo, pframe, len);
+ break;
+ case P2P_PROVISION_DISC_REQ:
+ DBG_88E("[%s] Got Provisioning Discovery Request Frame\n", __func__);
+ process_p2p_provdisc_req(pwdinfo, pframe, len);
+ memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+ /* 20110902 Kurt */
+ /* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+ rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
+ _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+ break;
+ case P2P_PROVISION_DISC_RESP:
+ /* Commented by Albert 20110707 */
+ /* Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
+ DBG_88E("[%s] Got Provisioning Discovery Response Frame\n", __func__);
+ /* Commented by Albert 20110426 */
+ /* The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
+ _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
+ process_p2p_provdisc_resp(pwdinfo, pframe);
+ _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+ break;
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(struct recv_frame *precv_frame)
+{
+ unsigned int ret = _FAIL;
+ u8 *pframe = precv_frame->rx_data;
+ u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ if (!memcmp(frame_body + 2, P2P_OUI, 4)) {
+ ret = on_action_public_p2p(precv_frame);
+ }
+
+ return ret;
+}
+
+static unsigned int on_action_public_default(struct recv_frame *precv_frame, u8 action)
+{
+ unsigned int ret = _FAIL;
+ u8 *pframe = precv_frame->rx_data;
+ u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+ u8 token;
+
+ token = frame_body[2];
+
+ if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+ goto exit;
+
+ ret = _SUCCESS;
+
+exit:
+ return ret;
+}
+
+unsigned int on_action_public(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ unsigned int ret = _FAIL;
+ u8 *pframe = precv_frame->rx_data;
+ u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+ u8 category, action;
+
+ /* check RA matches or not */
+ if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+ goto exit;
+
+ category = frame_body[0];
+ if (category != RTW_WLAN_CATEGORY_PUBLIC)
+ goto exit;
+
+ action = frame_body[1];
+ switch (action) {
+ case ACT_PUBLIC_VENDOR:
+ ret = on_action_public_vendor(precv_frame);
+ break;
+ default:
+ ret = on_action_public_default(precv_frame, action);
+ break;
+ }
+
+exit:
+ return ret;
+}
+
+unsigned int OnAction_ht(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ return _SUCCESS;
+}
+
+unsigned int OnAction_wmm(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ return _SUCCESS;
+}
+
+unsigned int OnAction_p2p(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_P2P
+ u8 *frame_body;
+ u8 category, OUI_Subtype;
+ u8 *pframe = precv_frame->rx_data;
+ uint len = precv_frame->len;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ DBG_88E("%s\n", __func__);
+
+ /* check RA matches or not */
+ if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+ return _SUCCESS;
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ category = frame_body[0];
+ if (category != RTW_WLAN_CATEGORY_P2P)
+ return _SUCCESS;
+
+ if (be32_to_cpu(*((__be32 *)(frame_body + 1))) != P2POUI)
+ return _SUCCESS;
+
+ len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+ OUI_Subtype = frame_body[5];
+
+ switch (OUI_Subtype) {
+ case P2P_NOTICE_OF_ABSENCE:
+ break;
+ case P2P_PRESENCE_REQUEST:
+ process_p2p_presence_req(pwdinfo, pframe, len);
+ break;
+ case P2P_PRESENCE_RESPONSE:
+ break;
+ case P2P_GO_DISC_REQUEST:
+ break;
+ default:
+ break;
+ }
+#endif /* CONFIG_88EU_P2P */
+ return _SUCCESS;
+}
+
+unsigned int OnAction(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ int i;
+ unsigned char category;
+ struct action_handler *ptable;
+ unsigned char *frame_body;
+ u8 *pframe = precv_frame->rx_data;
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ category = frame_body[0];
+
+ for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) {
+ ptable = &OnAction_tbl[i];
+ if (category == ptable->num)
+ ptable->func(padapter, precv_frame);
+ }
+ return _SUCCESS;
+}
+
+unsigned int DoReserved(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ return _SUCCESS;
+}
+
+struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+ struct xmit_frame *pmgntframe;
+ struct xmit_buf *pxmitbuf;
+
+ pmgntframe = rtw_alloc_xmitframe(pxmitpriv);
+ if (pmgntframe == NULL) {
+ DBG_88E("%s, alloc xmitframe fail\n", __func__);
+ return NULL;
+ }
+
+ pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+ if (pxmitbuf == NULL) {
+ DBG_88E("%s, alloc xmitbuf fail\n", __func__);
+ rtw_free_xmitframe(pxmitpriv, pmgntframe);
+ return NULL;
+ }
+ pmgntframe->frame_tag = MGNT_FRAMETAG;
+ pmgntframe->pxmitbuf = pxmitbuf;
+ pmgntframe->buf_addr = pxmitbuf->pbuf;
+ pxmitbuf->priv_data = pmgntframe;
+ return pmgntframe;
+}
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
+{
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ pmlmeext->tx_rate = rate;
+ DBG_88E("%s(): rate = %x\n", __func__, rate);
+}
+
+void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
+
+ pattrib->hdrlen = 24;
+ pattrib->nr_frags = 1;
+ pattrib->priority = 7;
+ pattrib->mac_id = 0;
+ pattrib->qsel = 0x12;
+
+ pattrib->pktlen = 0;
+
+ if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+ pattrib->raid = 6;/* b mode */
+ else
+ pattrib->raid = 5;/* a/g mode */
+
+ pattrib->encrypt = _NO_PRIVACY_;
+ pattrib->bswenc = false;
+
+ pattrib->qos_en = false;
+ pattrib->ht_en = false;
+ pattrib->bwmode = HT_CHANNEL_WIDTH_20;
+ pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ pattrib->sgi = false;
+
+ pattrib->seqnum = pmlmeext->mgnt_seq;
+
+ pattrib->retry_ctrl = true;
+}
+
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+ if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+ return;
+
+ rtw_hal_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+ s32 ret = _FAIL;
+ struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+ struct submit_ctx sctx;
+
+ if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+ return ret;
+
+ rtw_sctx_init(&sctx, timeout_ms);
+ pxmitbuf->sctx = &sctx;
+
+ ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
+
+ if (ret == _SUCCESS)
+ ret = rtw_sctx_wait(&sctx);
+
+ return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+ s32 ret = _FAIL;
+ u32 timeout_ms = 500;/* 500ms */
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+ return -1;
+
+ _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+ pxmitpriv->ack_tx = true;
+
+ pmgntframe->ack_report = 1;
+ if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
+ ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
+ }
+
+ pxmitpriv->ack_tx = false;
+ _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+
+ return ret;
+}
+
+static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+ u8 *ssid_ie;
+ int ssid_len_ori;
+ int len_diff = 0;
+
+ ssid_ie = rtw_get_ie(ies, WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+ if (ssid_ie && ssid_len_ori > 0) {
+ switch (hidden_ssid_mode) {
+ case 1: {
+ u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
+ u32 remain_len = 0;
+
+ remain_len = ies_len - (next_ie - ies);
+
+ ssid_ie[1] = 0;
+ memcpy(ssid_ie+2, next_ie, remain_len);
+ len_diff -= ssid_len_ori;
+
+ break;
+ }
+ case 2:
+ memset(&ssid_ie[2], 0, ssid_len_ori);
+ break;
+ default:
+ break;
+ }
+ }
+
+ return len_diff;
+}
+
+void issue_beacon(struct adapter *padapter, int timeout_ms)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned int rate_len;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+ u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL) {
+ DBG_88E("%s, alloc mgnt frame fail\n", __func__);
+ return;
+ }
+#if defined (CONFIG_88EU_AP_MODE)
+ spin_lock_bh(&pmlmepriv->bcn_update_lock);
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+ pattrib->qsel = 0x10;
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+ /* pmlmeext->mgnt_seq++; */
+ SetFrameSubType(pframe, WIFI_BEACON);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+#ifdef CONFIG_88EU_P2P
+ /* for P2P : Primary Device Type & Device Name */
+ u32 wpsielen = 0, insert_len = 0;
+ u8 *wpsie = NULL;
+ wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen > 0) {
+ uint wps_offset, remainder_ielen;
+ u8 *premainder_ie, *pframe_wscie;
+
+ wps_offset = (uint)(wpsie - cur_network->IEs);
+ premainder_ie = wpsie + wpsielen;
+ remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
+ pframe_wscie = pframe + wps_offset;
+ memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
+ pframe += (wps_offset + wpsielen);
+ pattrib->pktlen += (wps_offset + wpsielen);
+
+ /* now pframe is end of wsc ie, insert Primary Device Type & Device Name */
+ /* Primary Device Type */
+ /* Type: */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+ insert_len += 2;
+
+ /* Length: */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(0x0008);
+ insert_len += 2;
+
+ /* Value: */
+ /* Category ID */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ insert_len += 2;
+
+ /* OUI */
+ *(__be32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI);
+ insert_len += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ insert_len += 2;
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ insert_len += 2;
+
+ /* Length: */
+ *(__be16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len);
+ insert_len += 2;
+
+ /* Value: */
+ memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len);
+ insert_len += pwdinfo->device_name_len;
+
+ /* update wsc ie length */
+ *(pframe_wscie+1) = (wpsielen-2) + insert_len;
+
+ /* pframe move to end */
+ pframe += insert_len;
+ pattrib->pktlen += insert_len;
+
+ /* copy remainder_ie to pframe */
+ memcpy(pframe, premainder_ie, remainder_ielen);
+ pframe += remainder_ielen;
+ pattrib->pktlen += remainder_ielen;
+ } else
+#endif /* CONFIG_88EU_P2P */
+ {
+ int len_diff;
+ memcpy(pframe, cur_network->IEs, cur_network->IELength);
+ len_diff = update_hidden_ssid(
+ pframe+_BEACON_IE_OFFSET_
+ , cur_network->IELength-_BEACON_IE_OFFSET_
+ , pmlmeinfo->hidden_ssid_mode
+ );
+ pframe += (cur_network->IELength+len_diff);
+ pattrib->pktlen += (cur_network->IELength+len_diff);
+ }
+
+ {
+ u8 *wps_ie;
+ uint wps_ielen;
+ u8 sr = 0;
+ wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
+ pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
+ if (wps_ie && wps_ielen > 0)
+ rtw_get_wps_attr_content(wps_ie, wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+ if (sr != 0)
+ set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+ else
+ _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
+ }
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ u32 len;
+ len = build_beacon_p2p_ie(pwdinfo, pframe);
+
+ pframe += len;
+ pattrib->pktlen += len;
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ goto _issue_bcn;
+ }
+
+ /* below for ad-hoc mode */
+
+ /* timestamp will be inserted by hardware */
+ pframe += 8;
+ pattrib->pktlen += 8;
+
+ /* beacon interval: 2 bytes */
+
+ memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* capability info: 2 bytes */
+
+ memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* SSID */
+ pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+ /* supported rates... */
+ rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+ /* DS parameter set */
+ pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+ {
+ u8 erpinfo = 0;
+ u32 ATIMWindow;
+ /* IBSS Parameter Set... */
+ ATIMWindow = 0;
+ pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+ /* ERP IE */
+ pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+ }
+
+ /* EXTERNDED SUPPORTED RATE */
+ if (rate_len > 8)
+ pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+ /* todo:HT for adhoc */
+_issue_bcn:
+
+#if defined (CONFIG_88EU_AP_MODE)
+ pmlmepriv->update_bcn = false;
+
+ spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+
+ if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
+ DBG_88E("beacon frame too large\n");
+ return;
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ /* DBG_88E("issue bcn_sz=%d\n", pattrib->last_txcmdsz); */
+ if (timeout_ms > 0)
+ dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+ else
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned char *mac, *bssid;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+#if defined (CONFIG_88EU_AP_MODE)
+ u8 *pwps_ie;
+ uint wps_ielen;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+ unsigned int rate_len;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL) {
+ DBG_88E("%s, alloc mgnt frame fail\n", __func__);
+ return;
+ }
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ mac = myid(&(padapter->eeprompriv));
+ bssid = cur_network->MacAddress;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+ pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = pattrib->hdrlen;
+ pframe += pattrib->hdrlen;
+
+ if (cur_network->IELength > MAX_IE_SZ)
+ return;
+
+#if defined(CONFIG_88EU_AP_MODE)
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+ pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+ /* inerset & update wps_probe_resp_ie */
+ if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie && (wps_ielen > 0)) {
+ uint wps_offset, remainder_ielen;
+ u8 *premainder_ie;
+
+ wps_offset = (uint)(pwps_ie - cur_network->IEs);
+
+ premainder_ie = pwps_ie + wps_ielen;
+
+ remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
+
+ memcpy(pframe, cur_network->IEs, wps_offset);
+ pframe += wps_offset;
+ pattrib->pktlen += wps_offset;
+
+ wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
+ if ((wps_offset+wps_ielen+2) <= MAX_IE_SZ) {
+ memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
+ pframe += wps_ielen+2;
+ pattrib->pktlen += wps_ielen+2;
+ }
+
+ if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
+ memcpy(pframe, premainder_ie, remainder_ielen);
+ pframe += remainder_ielen;
+ pattrib->pktlen += remainder_ielen;
+ }
+ } else {
+ memcpy(pframe, cur_network->IEs, cur_network->IELength);
+ pframe += cur_network->IELength;
+ pattrib->pktlen += cur_network->IELength;
+ }
+ } else
+#endif
+ {
+ /* timestamp will be inserted by hardware */
+ pframe += 8;
+ pattrib->pktlen += 8;
+
+ /* beacon interval: 2 bytes */
+
+ memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* capability info: 2 bytes */
+
+ memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* below for ad-hoc mode */
+
+ /* SSID */
+ pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+ /* supported rates... */
+ rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+ /* DS parameter set */
+ pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+ u8 erpinfo = 0;
+ u32 ATIMWindow;
+ /* IBSS Parameter Set... */
+ /* ATIMWindow = cur->Configuration.ATIMWindow; */
+ ATIMWindow = 0;
+ pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+ /* ERP IE */
+ pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+ }
+
+ /* EXTERNDED SUPPORTED RATE */
+ if (rate_len > 8)
+ pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+ /* todo:HT for adhoc */
+ }
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
+ u32 len;
+ len = build_probe_resp_p2p_ie(pwdinfo, pframe);
+
+ pframe += len;
+ pattrib->pktlen += len;
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, int wait_ack)
+{
+ int ret = _FAIL;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned char *mac;
+ unsigned char bssrate[NumRates];
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ int bssrate_len = 0;
+ u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n"));
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ mac = myid(&(padapter->eeprompriv));
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ if (da) {
+ /* unicast probe request frame */
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+ } else {
+ /* broadcast probe request frame */
+ memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+ }
+
+ memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+ pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+
+ if (pssid)
+ pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
+ else
+ pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
+
+ get_rate_set(padapter, bssrate, &bssrate_len);
+
+ if (bssrate_len > 8) {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+ pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+ } else {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+ }
+
+ /* add wps_ie for wps2.0 */
+ if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
+ memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+ pframe += pmlmepriv->wps_probe_req_ie_len;
+ pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+ ("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
+
+ if (wait_ack) {
+ ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+ } else {
+ dump_mgntframe(padapter, pmgntframe);
+ ret = _SUCCESS;
+ }
+
+exit:
+ return ret;
+}
+
+inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+{
+ _issue_probereq(padapter, pssid, da, false);
+}
+
+int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da,
+ int try_cnt, int wait_ms)
+{
+ int ret;
+ int i = 0;
+ u32 start = jiffies;
+
+ do {
+ ret = _issue_probereq(padapter, pssid, da, wait_ms > 0 ? true : false);
+
+ i++;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+ break;
+
+ if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+ rtw_msleep_os(wait_ms);
+
+ } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+ if (ret != _FAIL) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+
+ if (try_cnt && wait_ms) {
+ if (da)
+ DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ else
+ DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ }
+exit:
+ return ret;
+}
+
+/* if psta == NULL, indiate we are station(client) now... */
+void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned int val32;
+ u16 val16;
+#ifdef CONFIG_88EU_AP_MODE
+ __le16 le_val16;
+#endif
+ int use_shared_key = 0;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_AUTH);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ if (psta) {/* for AP mode */
+#ifdef CONFIG_88EU_AP_MODE
+
+ memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ /* setting auth algo number */
+ val16 = (u16)psta->authalg;
+
+ if (status != _STATS_SUCCESSFUL_)
+ val16 = 0;
+
+ if (val16) {
+ le_val16 = cpu_to_le16(val16);
+ use_shared_key = 1;
+ } else {
+ le_val16 = 0;
+ }
+
+ pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+ /* setting auth seq number */
+ val16 = (u16)psta->auth_seq;
+ le_val16 = cpu_to_le16(val16);
+ pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+ /* setting status code... */
+ val16 = status;
+ le_val16 = cpu_to_le16(val16);
+ pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+ /* added challenging text... */
+ if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
+ pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
+#endif
+ } else {
+ __le32 le_tmp32;
+ __le16 le_tmp16;
+ memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+ /* setting auth algo number */
+ val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/* 0:OPEN System, 1:Shared key */
+ if (val16)
+ use_shared_key = 1;
+
+ /* setting IV for auth seq #3 */
+ if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+ val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+ le_tmp32 = cpu_to_le32(val32);
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
+
+ pattrib->iv_len = 4;
+ }
+
+ le_tmp16 = cpu_to_le16(val16);
+ pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
+
+ /* setting auth seq number */
+ val16 = pmlmeinfo->auth_seq;
+ le_tmp16 = cpu_to_le16(val16);
+ pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
+
+ /* setting status code... */
+ le_tmp16 = cpu_to_le16(status);
+ pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
+
+ /* then checking to see if sending challenging text... */
+ if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+ pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
+
+ SetPrivacy(fctrl);
+
+ pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pattrib->encrypt = _WEP40_;
+
+ pattrib->icv_len = 4;
+
+ pattrib->pktlen += pattrib->icv_len;
+ }
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
+ DBG_88E("%s\n", __func__);
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ struct xmit_frame *pmgntframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ struct pkt_attrib *pattrib;
+ unsigned char *pbuf, *pframe;
+ unsigned short val;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+ u8 *ie = pnetwork->IEs;
+ __le16 lestatus, leval;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ DBG_88E("%s\n", __func__);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+ memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+ SetFrameSubType(pwlanhdr, pkt_type);
+ else
+ return;
+
+ pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen += pattrib->hdrlen;
+ pframe += pattrib->hdrlen;
+
+ /* capability */
+ val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+ pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
+
+ lestatus = cpu_to_le16(status);
+ pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&lestatus, &(pattrib->pktlen));
+
+ leval = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+ pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&leval, &(pattrib->pktlen));
+
+ if (pstat->bssratelen <= 8) {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
+ } else {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
+ pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
+ }
+
+ if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+ uint ie_len = 0;
+
+ /* FILL HT CAP INFO IE */
+ pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+ if (pbuf && ie_len > 0) {
+ memcpy(pframe, pbuf, ie_len+2);
+ pframe += (ie_len+2);
+ pattrib->pktlen += (ie_len+2);
+ }
+
+ /* FILL HT ADD INFO IE */
+ pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+ if (pbuf && ie_len > 0) {
+ memcpy(pframe, pbuf, ie_len+2);
+ pframe += (ie_len+2);
+ pattrib->pktlen += (ie_len+2);
+ }
+ }
+
+ /* FILL WMM IE */
+ if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) {
+ uint ie_len = 0;
+ unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+ for (pbuf = ie + _BEACON_IE_OFFSET_;; pbuf += (ie_len + 2)) {
+ pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+ if (pbuf && !memcmp(pbuf+2, WMM_PARA_IE, 6)) {
+ memcpy(pframe, pbuf, ie_len+2);
+ pframe += (ie_len+2);
+ pattrib->pktlen += (ie_len+2);
+ break;
+ }
+
+ if ((pbuf == NULL) || (ie_len == 0))
+ break;
+ }
+ }
+
+ if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+
+ /* add WPS IE ie for wps 2.0 */
+ if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) {
+ memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
+
+ pframe += pmlmepriv->wps_assoc_resp_ie_len;
+ pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
+ }
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device)) {
+ u32 len;
+
+ len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
+
+ pframe += len;
+ pattrib->pktlen += len;
+ }
+#endif /* CONFIG_88EU_P2P */
+ pattrib->last_txcmdsz = pattrib->pktlen;
+ dump_mgntframe(padapter, pmgntframe);
+#endif
+}
+
+void issue_assocreq(struct adapter *padapter)
+{
+ int ret = _FAIL;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe, *p;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ __le16 le_tmp;
+ unsigned int i, j, ie_len, index = 0;
+ unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates];
+ struct ndis_802_11_var_ie *pIE;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ int bssrate_len = 0, sta_bssrate_len = 0;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 p2pie[255] = { 0x00 };
+ u16 p2pielen = 0;
+#endif /* CONFIG_88EU_P2P */
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+ memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ASSOCREQ);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ /* caps */
+
+ memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* listen interval */
+ /* todo: listen interval for power saving */
+ le_tmp = cpu_to_le16(3);
+ memcpy(pframe , (unsigned char *)&le_tmp, 2);
+ pframe += 2;
+ pattrib->pktlen += 2;
+
+ /* SSID */
+ pframe = rtw_set_ie(pframe, _SSID_IE_, pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
+
+ /* supported rate & extended supported rate */
+
+ /* Check if the AP's supported rates are also supported by STA. */
+ get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+
+ if (pmlmeext->cur_channel == 14)/* for JAPAN, channel 14 can only uses B Mode(CCK) */
+ sta_bssrate_len = 4;
+
+ for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+ if (pmlmeinfo->network.SupportedRates[i] == 0)
+ break;
+ DBG_88E("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
+ }
+
+ for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+ if (pmlmeinfo->network.SupportedRates[i] == 0)
+ break;
+
+ /* Check if the AP's supported rates are also supported by STA. */
+ for (j = 0; j < sta_bssrate_len; j++) {
+ /* Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
+ if ((pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
+ == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK))
+ break;
+ }
+
+ if (j == sta_bssrate_len) {
+ /* the rate is not supported by STA */
+ DBG_88E("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
+ } else {
+ /* the rate is supported by STA */
+ bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+ }
+ }
+
+ bssrate_len = index;
+ DBG_88E("bssrate_len=%d\n", bssrate_len);
+
+ if (bssrate_len == 0) {
+ rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+ rtw_free_xmitframe(pxmitpriv, pmgntframe);
+ goto exit; /* don't connect to AP if no joint supported rate */
+ }
+
+ if (bssrate_len > 8) {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+ pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+ } else {
+ pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+ }
+
+ /* RSN */
+ p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+ if (p != NULL)
+ pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, (p + 2), &(pattrib->pktlen));
+
+ /* HT caps */
+ if (padapter->mlmepriv.htpriv.ht_option) {
+ p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+ if ((p != NULL) && (!(is_ap_in_tkip(padapter)))) {
+ memcpy(&(pmlmeinfo->HT_caps), (p + 2), sizeof(struct HT_caps_element));
+
+ /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
+ if (pregpriv->cbw40_enable == 0)
+ pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &= cpu_to_le16(~(BIT(6) | BIT(1)));
+ else
+ pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(BIT(1));
+
+ /* todo: disable SM power save mode */
+ pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x000c);
+
+ rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+ switch (rf_type) {
+ case RF_1T1R:
+ if (pregpriv->rx_stbc)
+ pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
+ memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
+ break;
+ case RF_2T2R:
+ case RF_1T2R:
+ default:
+ if ((pregpriv->rx_stbc == 0x3) ||/* enable for 2.4/5 GHz */
+ ((pmlmeext->cur_wireless_mode & WIRELESS_11_24N) && (pregpriv->rx_stbc == 0x1)) || /* enable for 2.4GHz */
+ (pregpriv->wifi_spec == 1)) {
+ DBG_88E("declare supporting RX STBC\n");
+ pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
+ }
+ memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
+ break;
+ }
+ pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+ }
+ }
+
+ /* vendor specific IE, such as WPA, WMM, WPS */
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
+ (!memcmp(pIE->data, WMM_OUI, 4)) ||
+ (!memcmp(pIE->data, WPS_OUI, 4))) {
+ if (!padapter->registrypriv.wifi_spec) {
+ /* Commented by Kurt 20110629 */
+ /* In some older APs, WPS handshake */
+ /* would be fail if we append vender extensions informations to AP */
+ if (!memcmp(pIE->data, WPS_OUI, 4))
+ pIE->Length = 14;
+ }
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &(pattrib->pktlen));
+ }
+ break;
+ default:
+ break;
+ }
+ i += (pIE->Length + 2);
+ }
+
+ if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+
+#ifdef CONFIG_88EU_P2P
+
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
+ /* Should add the P2P IE in the association request frame. */
+ /* P2P OUI */
+
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20101109 */
+ /* According to the P2P Specification, the association request frame should contain 3 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. Extended Listen Timing */
+ /* 3. Device Info */
+ /* Commented by Albert 20110516 */
+ /* 4. P2P Interface */
+
+ /* P2P Capability */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+ else
+ p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+ /* Extended Listen Timing */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Availability Period */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ /* Availability Interval */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+ p2pielen += 2;
+
+ /* Device Info */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+ if ((pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
+ (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+ else
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC);
+
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ /* P2P Interface */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
+
+ /* Length: */
+ *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); /* P2P Device Address */
+ p2pielen += ETH_ALEN;
+
+ p2pie[p2pielen++] = 1; /* P2P Interface Address Count */
+
+ memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); /* P2P Interface Address List */
+ p2pielen += ETH_ALEN;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+ }
+
+#endif /* CONFIG_88EU_P2P */
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+ dump_mgntframe(padapter, pmgntframe);
+
+ ret = _SUCCESS;
+
+exit:
+ if (ret == _SUCCESS)
+ rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+ else
+ rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+
+ return;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+ int ret = _FAIL;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv;
+ struct mlme_ext_priv *pmlmeext;
+ struct mlme_ext_info *pmlmeinfo;
+
+ if (!padapter)
+ goto exit;
+
+ pxmitpriv = &(padapter->xmitpriv);
+ pmlmeext = &(padapter->mlmeextpriv);
+ pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+ pattrib->retry_ctrl = false;
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+ SetFrDs(fctrl);
+ else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+ SetToDs(fctrl);
+
+ if (power_mode)
+ SetPwrMgt(fctrl);
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ if (wait_ack) {
+ ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+ } else {
+ dump_mgntframe(padapter, pmgntframe);
+ ret = _SUCCESS;
+ }
+
+exit:
+ return ret;
+}
+
+/* when wait_ms > 0 , this function shoule be called at process context */
+/* da == NULL for station mode */
+int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+ int ret;
+ int i = 0;
+ u32 start = jiffies;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ /* da == NULL, assum it's null data for sta to ap*/
+ if (da == NULL)
+ da = get_my_bssid(&(pmlmeinfo->network));
+
+ do {
+ ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0 ? true : false);
+
+ i++;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+ break;
+
+ if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+ rtw_msleep_os(wait_ms);
+ } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+ if (ret != _FAIL) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+
+ if (try_cnt && wait_ms) {
+ if (da)
+ DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ else
+ DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ }
+exit:
+ return ret;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
+{
+ int ret = _FAIL;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ unsigned short *qc;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ DBG_88E("%s\n", __func__);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ pattrib->hdrlen += 2;
+ pattrib->qos_en = true;
+ pattrib->eosp = 1;
+ pattrib->ack_policy = 0;
+ pattrib->mdata = 0;
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+ SetFrDs(fctrl);
+ else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+ SetToDs(fctrl);
+
+ if (pattrib->mdata)
+ SetMData(fctrl);
+
+ qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+
+ SetPriority(qc, tid);
+
+ SetEOSP(qc, pattrib->eosp);
+
+ SetAckpolicy(qc, pattrib->ack_policy);
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ if (wait_ack) {
+ ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+ } else {
+ dump_mgntframe(padapter, pmgntframe);
+ ret = _SUCCESS;
+ }
+
+exit:
+ return ret;
+}
+
+/* when wait_ms > 0 , this function shoule be called at process context */
+/* da == NULL for station mode */
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+ int ret;
+ int i = 0;
+ u32 start = jiffies;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ /* da == NULL, assum it's null data for sta to ap*/
+ if (da == NULL)
+ da = get_my_bssid(&(pmlmeinfo->network));
+
+ do {
+ ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0 ? true : false);
+
+ i++;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+ break;
+
+ if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+ rtw_msleep_os(wait_ms);
+ } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+ if (ret != _FAIL) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+
+ if (try_cnt && wait_ms) {
+ if (da)
+ DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ else
+ DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ }
+exit:
+ return ret;
+}
+
+static int _issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ int ret = _FAIL;
+ __le16 le_tmp;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+#ifdef CONFIG_88EU_P2P
+ if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+ _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+ }
+#endif /* CONFIG_88EU_P2P */
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ goto exit;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+ pattrib->retry_ctrl = false;
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_DEAUTH);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ le_tmp = cpu_to_le16(reason);
+ pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ if (wait_ack) {
+ ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+ } else {
+ dump_mgntframe(padapter, pmgntframe);
+ ret = _SUCCESS;
+ }
+
+exit:
+ return ret;
+}
+
+int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+ DBG_88E("%s to %pM\n", __func__, da);
+ return _issue_deauth(padapter, da, reason, false);
+}
+
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+ int wait_ms)
+{
+ int ret;
+ int i = 0;
+ u32 start = jiffies;
+
+ do {
+ ret = _issue_deauth(padapter, da, reason, wait_ms > 0 ? true : false);
+
+ i++;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+ break;
+
+ if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+ rtw_msleep_os(wait_ms);
+ } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+ if (ret != _FAIL) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+
+ if (try_cnt && wait_ms) {
+ if (da)
+ DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ else
+ DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+ FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+ ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+ }
+exit:
+ return ret;
+}
+
+void issue_action_spct_ch_switch (struct adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ DBG_88E(FUNC_NDEV_FMT" ra =%pM, ch:%u, offset:%u\n",
+ FUNC_NDEV_ARG(padapter->pnetdev), ra, new_ch, ch_offset);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
+ memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ /* category, action */
+ {
+ u8 category, action;
+ category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
+ action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ }
+
+ pframe = rtw_set_ie_ch_switch (pframe, &(pattrib->pktlen), 0, new_ch, 0);
+ pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
+ hal_ch_offset_to_secondary_ch_offset(ch_offset));
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
+{
+ u8 category = RTW_WLAN_CATEGORY_BACK;
+ u16 start_seq;
+ u16 BA_para_set;
+ u16 reason_code;
+ u16 BA_timeout_value;
+ __le16 le_tmp;
+ u16 BA_starting_seqctrl = 0;
+ enum ht_cap_ampdu_factor max_rx_ampdu_factor;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ u8 *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct sta_info *psta;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+
+ DBG_88E("%s, category=%d, action=%d, status=%d\n", __func__, category, action, status);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ /* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+ if (category == 3) {
+ switch (action) {
+ case 0: /* ADDBA req */
+ do {
+ pmlmeinfo->dialogToken++;
+ } while (pmlmeinfo->dialogToken == 0);
+ pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
+
+ BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
+ le_tmp = cpu_to_le16(BA_para_set);
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+ BA_timeout_value = 5000;/* 5ms */
+ le_tmp = cpu_to_le16(BA_timeout_value);
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+ psta = rtw_get_stainfo(pstapriv, raddr);
+ if (psta != NULL) {
+ start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
+
+ DBG_88E("BA_starting_seqctrl=%d for TID=%d\n", start_seq, status & 0x07);
+
+ psta->BA_starting_seqctrl[status & 0x07] = start_seq;
+
+ BA_starting_seqctrl = start_seq << 4;
+ }
+ le_tmp = cpu_to_le16(BA_starting_seqctrl);
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+ break;
+ case 1: /* ADDBA rsp */
+ pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
+ BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f;
+ rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+ switch (max_rx_ampdu_factor) {
+ case MAX_AMPDU_FACTOR_64K:
+ BA_para_set |= 0x1000; /* 64 buffer size */
+ break;
+ case MAX_AMPDU_FACTOR_32K:
+ BA_para_set |= 0x0800; /* 32 buffer size */
+ break;
+ case MAX_AMPDU_FACTOR_16K:
+ BA_para_set |= 0x0400; /* 16 buffer size */
+ break;
+ case MAX_AMPDU_FACTOR_8K:
+ BA_para_set |= 0x0200; /* 8 buffer size */
+ break;
+ default:
+ BA_para_set |= 0x1000; /* 64 buffer size */
+ break;
+ }
+
+ if (pregpriv->ampdu_amsdu == 0)/* disabled */
+ BA_para_set = BA_para_set & ~BIT(0);
+ else if (pregpriv->ampdu_amsdu == 1)/* enabled */
+ BA_para_set = BA_para_set | BIT(0);
+ le_tmp = cpu_to_le16(BA_para_set);
+
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
+ break;
+ case 2:/* DELBA */
+ BA_para_set = (status & 0x1F) << 3;
+ le_tmp = cpu_to_le16(BA_para_set);
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+ reason_code = 37;/* Requested from peer STA as it does not want to use the mechanism */
+ le_tmp = cpu_to_le16(reason_code);
+ pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+ break;
+ default:
+ break;
+ }
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_action_BSSCoexistPacket(struct adapter *padapter)
+{
+ struct list_head *plist, *phead;
+ unsigned char category, action;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct wlan_network *pnetwork = NULL;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct __queue *queue = &(pmlmepriv->scanned_queue);
+ u8 InfoContent[16] = {0};
+ u8 ICS[8][15];
+ if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+ return;
+
+ if (pmlmeinfo->bwmode_updated)
+ return;
+
+ DBG_88E("%s\n", __func__);
+
+ category = RTW_WLAN_CATEGORY_PUBLIC;
+ action = ACT_PUBLIC_BSSCOEXIST;
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+ /* */
+ if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+ u8 iedata = 0;
+
+ iedata |= BIT(2);/* 20 MHz BSS Width Request */
+
+ pframe = rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen));
+ }
+
+ /* */
+ memset(ICS, 0, sizeof(ICS));
+ if (pmlmepriv->num_sta_no_ht > 0) {
+ int i;
+
+ spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+
+ phead = get_list_head(queue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ int len;
+ u8 *p;
+ struct wlan_bssid_ex *pbss_network;
+
+ pnetwork = container_of(plist, struct wlan_network, list);
+
+ plist = plist->next;
+
+ pbss_network = (struct wlan_bssid_ex *)&pnetwork->network;
+
+ p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+ if ((p == NULL) || (len == 0)) { /* non-HT */
+ if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
+ continue;
+
+ ICS[0][pbss_network->Configuration.DSConfig] = 1;
+
+ if (ICS[0][0] == 0)
+ ICS[0][0] = 1;
+ }
+ }
+ spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+
+ for (i = 0; i < 8; i++) {
+ if (ICS[i][0] == 1) {
+ int j, k = 0;
+
+ InfoContent[k] = i;
+ /* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+ k++;
+
+ for (j = 1; j <= 14; j++) {
+ if (ICS[i][j] == 1) {
+ if (k < 16) {
+ InfoContent[k] = j; /* channel number */
+ /* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
+ k++;
+ }
+ }
+ }
+
+ pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
+ }
+ }
+ }
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr)
+{
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct sta_info *psta = NULL;
+ /* struct recv_reorder_ctrl *preorder_ctrl; */
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u16 tid;
+
+ if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+ if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+ return _SUCCESS;
+
+ psta = rtw_get_stainfo(pstapriv, addr);
+ if (psta == NULL)
+ return _SUCCESS;
+
+ if (initiator == 0) { /* recipient */
+ for (tid = 0; tid < MAXTID; tid++) {
+ if (psta->recvreorder_ctrl[tid].enable) {
+ DBG_88E("rx agg disable tid(%d)\n", tid);
+ issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+ psta->recvreorder_ctrl[tid].enable = false;
+ psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+ }
+ }
+ } else if (initiator == 1) { /* originator */
+ for (tid = 0; tid < MAXTID; tid++) {
+ if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+ DBG_88E("tx agg disable tid(%d)\n", tid);
+ issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+ psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+ psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+ }
+ }
+ }
+
+ return _SUCCESS;
+}
+
+unsigned int send_beacon(struct adapter *padapter)
+{
+ u8 bxmitok = false;
+ int issue = 0;
+ int poll = 0;
+
+ u32 start = jiffies;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+ do {
+ issue_beacon(padapter, 100);
+ issue++;
+ do {
+ rtw_yield_os();
+ rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+ poll++;
+ } while ((poll%10) != 0 && !bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+ } while (!bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+ if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+ return _FAIL;
+ if (!bxmitok) {
+ DBG_88E("%s fail! %u ms\n", __func__, rtw_get_passing_time_ms(start));
+ return _FAIL;
+ } else {
+ u32 passing_time = rtw_get_passing_time_ms(start);
+
+ if (passing_time > 100 || issue > 3)
+ DBG_88E("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
+ return _SUCCESS;
+ }
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void site_survey(struct adapter *padapter)
+{
+ unsigned char survey_channel = 0, val8;
+ enum rt_scan_type ScanType = SCAN_PASSIVE;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u32 initialgain = 0;
+
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
+ if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+ survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+ } else {
+ survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+ }
+ ScanType = SCAN_ACTIVE;
+ } else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
+ /* Commented by Albert 2011/06/03 */
+ /* The driver is in the find phase, it should go through the social channel. */
+ int ch_set_idx;
+ survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
+ ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
+ if (ch_set_idx >= 0)
+ ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
+ else
+ ScanType = SCAN_ACTIVE;
+ } else
+#endif /* CONFIG_88EU_P2P */
+ {
+ struct rtw_ieee80211_channel *ch;
+ if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
+ ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+ survey_channel = ch->hw_value;
+ ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+ }
+ }
+
+ if (survey_channel != 0) {
+ /* PAUSE 4-AC Queue when site_survey */
+ /* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+ /* val8 |= 0x0f; */
+ /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+ if (pmlmeext->sitesurvey_res.channel_idx == 0)
+ set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ else
+ SelectChannel(padapter, survey_channel);
+
+ if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
+ #ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
+ rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
+ issue_probereq_p2p(padapter, NULL);
+ issue_probereq_p2p(padapter, NULL);
+ issue_probereq_p2p(padapter, NULL);
+ } else
+ #endif /* CONFIG_88EU_P2P */
+ {
+ int i;
+ for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+ if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
+ /* todo: to issue two probe req??? */
+ issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+ /* rtw_msleep_os(SURVEY_TO>>1); */
+ issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+ }
+ }
+
+ if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+ /* todo: to issue two probe req??? */
+ issue_probereq(padapter, NULL, NULL);
+ /* rtw_msleep_os(SURVEY_TO>>1); */
+ issue_probereq(padapter, NULL, NULL);
+ }
+ }
+ }
+
+ set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+ } else {
+ /* channel number is 0 or this channel is not valid. */
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
+ if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
+ /* Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
+ /* This will let the following flow to run the scanning end. */
+ rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+ }
+ }
+
+ if (rtw_p2p_findphase_ex_is_needed(pwdinfo)) {
+ /* Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
+ set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
+ pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+ initialgain = 0xff; /* restore RX GAIN */
+ rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+ /* turn on dynamic functions */
+ Restore_DM_Func_Flag(padapter);
+ /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */
+
+ _set_timer(&pwdinfo->find_phase_timer, (u32)((u32)(pwdinfo->listen_dwell) * 100));
+ } else
+#endif /* CONFIG_88EU_P2P */
+ {
+ /* 20100721:Interrupt scan operation here. */
+ /* For SW antenna diversity before link, it needs to switch to another antenna and scan again. */
+ /* It compares the scan result and select beter one to do connection. */
+ if (rtw_hal_antdiv_before_linked(padapter)) {
+ pmlmeext->sitesurvey_res.bss_cnt = 0;
+ pmlmeext->sitesurvey_res.channel_idx = -1;
+ pmlmeext->chan_scan_time = SURVEY_TO / 2;
+ set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+ return;
+ }
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+ rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+ rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+#endif /* CONFIG_88EU_P2P */
+
+ pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+ /* switch back to the original channel */
+
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN))
+ set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ else
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+#endif /* CONFIG_88EU_P2P */
+
+ /* flush 4-AC Queue after site_survey */
+ /* val8 = 0; */
+ /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+
+ /* config MSR */
+ Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+ initialgain = 0xff; /* restore RX GAIN */
+ rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+ /* turn on dynamic functions */
+ Restore_DM_Func_Flag(padapter);
+ /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+
+ if (is_client_associated_to_ap(padapter))
+ issue_nulldata(padapter, NULL, 0, 3, 500);
+
+ val8 = 0; /* survey done */
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+ report_surveydone_event(padapter);
+
+ pmlmeext->chan_scan_time = SURVEY_TO;
+ pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+ issue_action_BSSCoexistPacket(padapter);
+ issue_action_BSSCoexistPacket(padapter);
+ issue_action_BSSCoexistPacket(padapter);
+ }
+ }
+ return;
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+{
+ int i;
+ u32 len;
+ u8 *p;
+ u16 val16, subtype;
+ u8 *pframe = precv_frame->rx_data;
+ u32 packet_len = precv_frame->len;
+ u8 ie_offset;
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ __le32 le32_tmp;
+
+ len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ if (len > MAX_IE_SZ)
+ return _FAIL;
+
+ memset(bssid, 0, sizeof(struct wlan_bssid_ex));
+
+ subtype = GetFrameSubType(pframe);
+
+ if (subtype == WIFI_BEACON) {
+ bssid->Reserved[0] = 1;
+ ie_offset = _BEACON_IE_OFFSET_;
+ } else {
+ /* FIXME : more type */
+ if (subtype == WIFI_PROBEREQ) {
+ ie_offset = _PROBEREQ_IE_OFFSET_;
+ bssid->Reserved[0] = 2;
+ } else if (subtype == WIFI_PROBERSP) {
+ ie_offset = _PROBERSP_IE_OFFSET_;
+ bssid->Reserved[0] = 3;
+ } else {
+ bssid->Reserved[0] = 0;
+ ie_offset = _FIXED_IE_LENGTH_;
+ }
+ }
+
+ bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+ /* below is to copy the information element */
+ bssid->IELength = len;
+ memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+ /* get the signal strength */
+ bssid->Rssi = precv_frame->attrib.phy_info.recvpower; /* in dBM.raw data */
+ bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
+ bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
+ rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA, &bssid->PhyInfo.Optimum_antenna);
+
+ /* checking SSID */
+ p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+ if (p == NULL) {
+ DBG_88E("marc: cannot find SSID for survey event\n");
+ return _FAIL;
+ }
+
+ if (*(p + 1)) {
+ if (len > NDIS_802_11_LENGTH_SSID) {
+ DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+ return _FAIL;
+ }
+ memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+ bssid->Ssid.SsidLength = *(p + 1);
+ } else {
+ bssid->Ssid.SsidLength = 0;
+ }
+
+ memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+ /* checking rate info... */
+ i = 0;
+ p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+ if (p != NULL) {
+ if (len > NDIS_802_11_LENGTH_RATES_EX) {
+ DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+ return _FAIL;
+ }
+ memcpy(bssid->SupportedRates, (p + 2), len);
+ i = len;
+ }
+
+ p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+ if (p != NULL) {
+ if (len > (NDIS_802_11_LENGTH_RATES_EX-i)) {
+ DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+ return _FAIL;
+ }
+ memcpy(bssid->SupportedRates + i, (p + 2), len);
+ }
+
+ /* todo: */
+ bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+
+ if (bssid->IELength < 12)
+ return _FAIL;
+
+ /* Checking for DSConfig */
+ p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+
+ bssid->Configuration.DSConfig = 0;
+ bssid->Configuration.Length = 0;
+
+ if (p) {
+ bssid->Configuration.DSConfig = *(p + 2);
+ } else {/* In 5G, some ap do not have DSSET IE */
+ /* checking HT info for channel */
+ p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
+ if (p) {
+ struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+ bssid->Configuration.DSConfig = HT_info->primary_channel;
+ } else { /* use current channel */
+ bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
+ }
+ }
+
+ memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+ bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+
+ val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+ if (val16 & BIT(0)) {
+ bssid->InfrastructureMode = Ndis802_11Infrastructure;
+ memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+ } else {
+ bssid->InfrastructureMode = Ndis802_11IBSS;
+ memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
+ }
+
+ if (val16 & BIT(4))
+ bssid->Privacy = 1;
+ else
+ bssid->Privacy = 0;
+
+ bssid->Configuration.ATIMWindow = 0;
+
+ /* 20/40 BSS Coexistence check */
+ if ((pregistrypriv->wifi_spec == 1) && (!pmlmeinfo->bwmode_updated)) {
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
+ if (p && len > 0) {
+ struct HT_caps_element *pHT_caps;
+ pHT_caps = (struct HT_caps_element *)(p + 2);
+
+ if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info)&BIT(14))
+ pmlmepriv->num_FortyMHzIntolerant++;
+ } else {
+ pmlmepriv->num_sta_no_ht++;
+ }
+ }
+
+ /* mark bss info receiving from nearby channel as SignalQuality 101 */
+ if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+ bssid->PhyInfo.SignalQuality = 101;
+ return _SUCCESS;
+}
+
+void start_create_ibss(struct adapter *padapter)
+{
+ unsigned short caps;
+ u8 val8;
+ u8 join_type;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+ pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+ pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+ /* update wireless mode */
+ update_wireless_mode(padapter);
+
+ /* udpate capability */
+ caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+ update_capinfo(padapter, caps);
+ if (caps&cap_IBSS) {/* adhoc master */
+ val8 = 0xcf;
+ rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+ /* switch channel */
+ /* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+
+ beacon_timing_control(padapter);
+
+ /* set msr to WIFI_FW_ADHOC_STATE */
+ pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+ Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+ /* issue beacon */
+ if (send_beacon(padapter) == _FAIL) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n"));
+
+ report_join_res(padapter, -1);
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ } else {
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+ join_type = 0;
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+ report_join_res(padapter, 1);
+ pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+ rtw_indicate_connect(padapter);
+ }
+ } else {
+ DBG_88E("start_create_ibss, invalid cap:%x\n", caps);
+ return;
+ }
+ /* update bc/mc sta_info */
+ update_bmc_sta(padapter);
+}
+
+void start_clnt_join(struct adapter *padapter)
+{
+ unsigned short caps;
+ u8 val8;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+ int beacon_timeout;
+
+ pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+ pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+ /* update wireless mode */
+ update_wireless_mode(padapter);
+
+ /* udpate capability */
+ caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+ update_capinfo(padapter, caps);
+ if (caps&cap_ESS) {
+ Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+ val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+ /* switch channel */
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+ /* here wait for receiving the beacon to start auth */
+ /* and enable a timer */
+ beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+ set_link_timer(pmlmeext, beacon_timeout);
+ _set_timer(&padapter->mlmepriv.assoc_timer,
+ (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout);
+
+ pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+ } else if (caps&cap_IBSS) { /* adhoc client */
+ Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+ val8 = 0xcf;
+ rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+ /* switch channel */
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+ beacon_timing_control(padapter);
+
+ pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+ report_join_res(padapter, 1);
+ } else {
+ return;
+ }
+}
+
+void start_clnt_auth(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
+ pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
+
+ pmlmeinfo->auth_seq = 1;
+ pmlmeinfo->reauth_count = 0;
+ pmlmeinfo->reassoc_count = 0;
+ pmlmeinfo->link_count = 0;
+ pmlmeext->retry = 0;
+
+ /* Because of AP's not receiving deauth before */
+ /* AP may: 1)not response auth or 2)deauth us after link is complete */
+ /* issue deauth before issuing auth to deal with the situation */
+ /* Commented by Albert 2012/07/21 */
+ /* For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
+ issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
+
+ DBG_88E_LEVEL(_drv_info_, "start auth\n");
+ issue_auth(padapter, NULL, 0);
+
+ set_link_timer(pmlmeext, REAUTH_TO);
+}
+
+void start_clnt_assoc(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+ pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+ issue_assocreq(padapter);
+
+ set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ /* check A3 */
+ if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+ return _SUCCESS;
+
+ DBG_88E("%s\n", __func__);
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+ if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ report_del_sta_event(padapter, MacAddr, reason);
+ } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ report_join_res(padapter, -2);
+ }
+ }
+ return _SUCCESS;
+}
+
+static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
+{
+ struct registry_priv *pregistrypriv;
+ struct mlme_ext_priv *pmlmeext;
+ struct rt_channel_info *chplan_new;
+ u8 channel;
+ u8 i;
+
+ pregistrypriv = &padapter->registrypriv;
+ pmlmeext = &padapter->mlmeextpriv;
+
+ /* Adjust channel plan by AP Country IE */
+ if (pregistrypriv->enable80211d &&
+ (!pmlmeext->update_channel_plan_by_ap_done)) {
+ u8 *ie, *p;
+ u32 len;
+ struct rt_channel_plan chplan_ap;
+ struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
+ u8 country[4];
+ u8 fcn; /* first channel number */
+ u8 noc; /* number of channel */
+ u8 j, k;
+
+ ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (!ie)
+ return;
+ if (len < 6)
+ return;
+ ie += 2;
+ p = ie;
+ ie += len;
+
+ memset(country, 0, 4);
+ memcpy(country, p, 3);
+ p += 3;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+ ("%s: 802.11d country =%s\n", __func__, country));
+
+ i = 0;
+ while ((ie - p) >= 3) {
+ fcn = *(p++);
+ noc = *(p++);
+ p++;
+
+ for (j = 0; j < noc; j++) {
+ if (fcn <= 14)
+ channel = fcn + j; /* 2.4 GHz */
+ else
+ channel = fcn + j*4; /* 5 GHz */
+
+ chplan_ap.Channel[i++] = channel;
+ }
+ }
+ chplan_ap.Len = i;
+
+ memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+
+ memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+ chplan_new = pmlmeext->channel_set;
+
+ i = 0;
+ j = 0;
+ k = 0;
+ if (pregistrypriv->wireless_mode & WIRELESS_11G) {
+ do {
+ if ((i == MAX_CHANNEL_NUM) ||
+ (chplan_sta[i].ChannelNum == 0) ||
+ (chplan_sta[i].ChannelNum > 14))
+ break;
+
+ if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+ break;
+
+ if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+ chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+ chplan_new[k].ScanType = SCAN_ACTIVE;
+ i++;
+ j++;
+ k++;
+ } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+ chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+ chplan_new[k].ScanType = SCAN_PASSIVE;
+ i++;
+ k++;
+ } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+ chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+ chplan_new[k].ScanType = SCAN_ACTIVE;
+ j++;
+ k++;
+ }
+ } while (1);
+
+ /* change AP not support channel to Passive scan */
+ while ((i < MAX_CHANNEL_NUM) &&
+ (chplan_sta[i].ChannelNum != 0) &&
+ (chplan_sta[i].ChannelNum <= 14)) {
+ chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+ chplan_new[k].ScanType = SCAN_PASSIVE;
+ i++;
+ k++;
+ }
+
+ /* add channel AP supported */
+ while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+ chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+ chplan_new[k].ScanType = SCAN_ACTIVE;
+ j++;
+ k++;
+ }
+ } else {
+ /* keep original STA 2.4G channel plan */
+ while ((i < MAX_CHANNEL_NUM) &&
+ (chplan_sta[i].ChannelNum != 0) &&
+ (chplan_sta[i].ChannelNum <= 14)) {
+ chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+ chplan_new[k].ScanType = chplan_sta[i].ScanType;
+ i++;
+ k++;
+ }
+
+ /* skip AP 2.4G channel plan */
+ while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+ j++;
+ }
+
+ /* keep original STA 5G channel plan */
+ while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+ chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+ chplan_new[k].ScanType = chplan_sta[i].ScanType;
+ i++;
+ k++;
+ }
+
+ pmlmeext->update_channel_plan_by_ap_done = 1;
+ }
+
+ /* If channel is used by AP, set channel scan type to active */
+ channel = bssid->Configuration.DSConfig;
+ chplan_new = pmlmeext->channel_set;
+ i = 0;
+ while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
+ if (chplan_new[i].ChannelNum == channel) {
+ if (chplan_new[i].ScanType == SCAN_PASSIVE) {
+ chplan_new[i].ScanType = SCAN_ACTIVE;
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+ ("%s: change channel %d scan type from passive to active\n",
+ __func__, channel));
+ }
+ break;
+ }
+ i++;
+ }
+}
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ struct cmd_obj *pcmd_obj;
+ u8 *pevtcmd;
+ u32 cmdsz;
+ struct survey_event *psurvey_evt;
+ struct C2HEvent_Header *pc2h_evt_hdr;
+ struct mlme_ext_priv *pmlmeext;
+ struct cmd_priv *pcmdpriv;
+ /* u8 *pframe = precv_frame->rx_data; */
+ /* uint len = precv_frame->len; */
+
+ if (!padapter)
+ return;
+
+ pmlmeext = &padapter->mlmeextpriv;
+ pcmdpriv = &padapter->cmdpriv;
+
+ pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd_obj == NULL)
+ return;
+
+ cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+ pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+ if (pevtcmd == NULL) {
+ kfree(pcmd_obj);
+ return;
+ }
+
+ INIT_LIST_HEAD(&pcmd_obj->list);
+
+ pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+ pcmd_obj->cmdsz = cmdsz;
+ pcmd_obj->parmbuf = pevtcmd;
+
+ pcmd_obj->rsp = NULL;
+ pcmd_obj->rspsz = 0;
+
+ pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+ pc2h_evt_hdr->len = sizeof(struct survey_event);
+ pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
+ pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+ psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+
+ if (collect_bss_info(padapter, precv_frame, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
+ kfree(pcmd_obj);
+ kfree(pevtcmd);
+ return;
+ }
+
+ process_80211d(padapter, &psurvey_evt->bss);
+
+ rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+ pmlmeext->sitesurvey_res.bss_cnt++;
+
+ return;
+}
+
+void report_surveydone_event(struct adapter *padapter)
+{
+ struct cmd_obj *pcmd_obj;
+ u8 *pevtcmd;
+ u32 cmdsz;
+ struct surveydone_event *psurveydone_evt;
+ struct C2HEvent_Header *pc2h_evt_hdr;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd_obj == NULL)
+ return;
+
+ cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+ pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+ if (pevtcmd == NULL) {
+ kfree(pcmd_obj);
+ return;
+ }
+
+ INIT_LIST_HEAD(&pcmd_obj->list);
+
+ pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+ pcmd_obj->cmdsz = cmdsz;
+ pcmd_obj->parmbuf = pevtcmd;
+
+ pcmd_obj->rsp = NULL;
+ pcmd_obj->rspsz = 0;
+
+ pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+ pc2h_evt_hdr->len = sizeof(struct surveydone_event);
+ pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
+ pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+ psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+ psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
+
+ DBG_88E("survey done event(%x)\n", psurveydone_evt->bss_cnt);
+
+ rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+ return;
+}
+
+void report_join_res(struct adapter *padapter, int res)
+{
+ struct cmd_obj *pcmd_obj;
+ u8 *pevtcmd;
+ u32 cmdsz;
+ struct joinbss_event *pjoinbss_evt;
+ struct C2HEvent_Header *pc2h_evt_hdr;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd_obj == NULL)
+ return;
+
+ cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+ pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+ if (pevtcmd == NULL) {
+ kfree(pcmd_obj);
+ return;
+ }
+
+ INIT_LIST_HEAD(&pcmd_obj->list);
+
+ pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+ pcmd_obj->cmdsz = cmdsz;
+ pcmd_obj->parmbuf = pevtcmd;
+
+ pcmd_obj->rsp = NULL;
+ pcmd_obj->rspsz = 0;
+
+ pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+ pc2h_evt_hdr->len = sizeof(struct joinbss_event);
+ pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+ pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+ pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+ memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+ pjoinbss_evt->network.join_res = res;
+ pjoinbss_evt->network.aid = res;
+
+ DBG_88E("report_join_res(%d)\n", res);
+
+ rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+
+ rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+ return;
+}
+
+void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+ struct cmd_obj *pcmd_obj;
+ u8 *pevtcmd;
+ u32 cmdsz;
+ struct sta_info *psta;
+ int mac_id;
+ struct stadel_event *pdel_sta_evt;
+ struct C2HEvent_Header *pc2h_evt_hdr;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd_obj == NULL)
+ return;
+
+ cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+ pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+ if (pevtcmd == NULL) {
+ kfree(pcmd_obj);
+ return;
+ }
+
+ INIT_LIST_HEAD(&pcmd_obj->list);
+
+ pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+ pcmd_obj->cmdsz = cmdsz;
+ pcmd_obj->parmbuf = pevtcmd;
+
+ pcmd_obj->rsp = NULL;
+ pcmd_obj->rspsz = 0;
+
+ pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+ pc2h_evt_hdr->len = sizeof(struct stadel_event);
+ pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
+ pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+ pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+ memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+ memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+
+ psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+ if (psta)
+ mac_id = (int)psta->mac_id;
+ else
+ mac_id = (-1);
+
+ pdel_sta_evt->mac_id = mac_id;
+
+ DBG_88E("report_del_sta_event: delete STA, mac_id =%d\n", mac_id);
+
+ rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+ return;
+}
+
+void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx)
+{
+ struct cmd_obj *pcmd_obj;
+ u8 *pevtcmd;
+ u32 cmdsz;
+ struct stassoc_event *padd_sta_evt;
+ struct C2HEvent_Header *pc2h_evt_hdr;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+ pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (pcmd_obj == NULL)
+ return;
+
+ cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+ pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+ if (pevtcmd == NULL) {
+ kfree(pcmd_obj);
+ return;
+ }
+
+ INIT_LIST_HEAD(&pcmd_obj->list);
+
+ pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+ pcmd_obj->cmdsz = cmdsz;
+ pcmd_obj->parmbuf = pevtcmd;
+
+ pcmd_obj->rsp = NULL;
+ pcmd_obj->rspsz = 0;
+
+ pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+ pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+ pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
+ pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+ padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+ memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+ padd_sta_evt->cam_id = cam_idx;
+
+ DBG_88E("report_add_sta_event: add STA\n");
+
+ rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+ return;
+}
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta)
+{
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ /* ERP */
+ VCS_update(padapter, psta);
+
+ /* HT */
+ if (pmlmepriv->htpriv.ht_option) {
+ psta->htpriv.ht_option = true;
+
+ psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+ if (support_short_GI(padapter, &(pmlmeinfo->HT_caps)))
+ psta->htpriv.sgi = true;
+
+ psta->qos_option = true;
+ } else {
+ psta->htpriv.ht_option = false;
+
+ psta->htpriv.ampdu_enable = false;
+
+ psta->htpriv.sgi = false;
+ psta->qos_option = false;
+ }
+ psta->htpriv.bwmode = pmlmeext->cur_bwmode;
+ psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
+
+ psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+ psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+ /* QoS */
+ if (pmlmepriv->qospriv.qos_option)
+ psta->qos_option = true;
+
+ psta->state = _FW_LINKED;
+}
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
+{
+ struct sta_info *psta, *psta_bmc;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 join_type;
+ u16 media_status;
+
+ if (join_res < 0) {
+ join_type = 1;
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+ /* restore to initial setting. */
+ update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+ goto exit_mlmeext_joinbss_event_callback;
+ }
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+ /* for bc/mc */
+ psta_bmc = rtw_get_bcmc_stainfo(padapter);
+ if (psta_bmc) {
+ pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
+ update_bmc_sta_support_rate(padapter, psta_bmc->mac_id);
+ Update_RA_Entry(padapter, psta_bmc->mac_id);
+ }
+ }
+
+ /* turn on dynamic functions */
+ Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+
+ /* update IOT-releated issue */
+ update_IOT_info(padapter);
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+
+ /* BCN interval */
+ rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+
+ /* udpate capability */
+ update_capinfo(padapter, pmlmeinfo->capability);
+
+ /* WMM, Update EDCA param */
+ WMMOnAssocRsp(padapter);
+
+ /* HT */
+ HTOnAssocRsp(padapter);
+
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+ psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+ if (psta) { /* only for infra. mode */
+ pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+ psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+ /* set per sta rate after updating HT cap. */
+ set_sta_rate(padapter, psta);
+ rtw_hal_set_hwreg(padapter, HW_VAR_TX_RPT_MAX_MACID, (u8 *)&psta->mac_id);
+ media_status = (psta->mac_id<<8)|1; /* MACID|OPMODE: 1 means connect */
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
+ }
+
+ join_type = 2;
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+ /* correcting TSF */
+ correct_TSF(padapter, pmlmeext);
+ }
+ rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+
+exit_mlmeext_joinbss_event_callback:
+
+ DBG_88E("=>%s\n", __func__);
+}
+
+void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta)
+{
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 join_type;
+
+ DBG_88E("%s\n", __func__);
+
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+ if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {/* adhoc master or sta_count>1 */
+ /* nothing to do */
+ } else { /* adhoc client */
+ /* correcting TSF */
+ correct_TSF(padapter, pmlmeext);
+
+ /* start beacon */
+ if (send_beacon(padapter) == _FAIL) {
+ pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
+ pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
+ return;
+ }
+ pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+ }
+
+ join_type = 2;
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+ }
+
+ pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+ /* rate radaptive */
+ Update_RA_Entry(padapter, psta->mac_id);
+
+ /* update adhoc sta_info */
+ update_sta_info(padapter, psta);
+}
+
+void mlmeext_sta_del_event_callback(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter)) {
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+ /* restore to initial setting. */
+ update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+ /* switch to the 20M Hz mode after disconnect */
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+ /* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+ flush_all_cam_entry(padapter);
+
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+ /* set MSR to no link state -> infra. mode */
+ Set_MSR(padapter, _HW_STATE_STATION_);
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+ }
+}
+
+/****************************************************************************
+
+Following are the functions for the timer handlers
+
+*****************************************************************************/
+void _linked_rx_signal_strehgth_display(struct adapter *padapter);
+void _linked_rx_signal_strehgth_display(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 mac_id;
+ int UndecoratedSmoothedPWDB;
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+ mac_id = 0;
+ else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
+ mac_id = 2;
+
+ rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &mac_id);
+
+ rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
+ DBG_88E("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB);
+}
+
+static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta)
+{
+ u8 ret = false;
+
+ if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) &&
+ sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
+ sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
+ ret = false;
+ else
+ ret = true;
+
+ sta_update_last_rx_pkts(psta);
+
+ return ret;
+}
+
+void linked_status_chk(struct adapter *padapter)
+{
+ u32 i;
+ struct sta_info *psta;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ if (padapter->bRxRSSIDisplay)
+ _linked_rx_signal_strehgth_display(padapter);
+
+ rtw_hal_sreset_linked_status_check(padapter);
+
+ if (is_client_associated_to_ap(padapter)) {
+ /* linked infrastructure client mode */
+
+ int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+ int rx_chk_limit;
+
+ rx_chk_limit = 4;
+ psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+ if (psta != NULL) {
+ bool is_p2p_enable = false;
+ #ifdef CONFIG_88EU_P2P
+ is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
+ #endif
+
+ if (!chk_ap_is_alive(padapter, psta))
+ rx_chk = _FAIL;
+
+ if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+ tx_chk = _FAIL;
+
+ if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
+ u8 backup_oper_channel = 0;
+
+ /* switch to correct channel of current network before issue keep-alive frames */
+ if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+ backup_oper_channel = rtw_get_oper_ch(padapter);
+ SelectChannel(padapter, pmlmeext->cur_channel);
+ }
+
+ if (rx_chk != _SUCCESS)
+ issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
+
+ if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
+ tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
+ /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
+ if (tx_chk == _SUCCESS && !is_p2p_enable)
+ rx_chk = _SUCCESS;
+ }
+
+ /* back to the original operation channel */
+ if (backup_oper_channel > 0)
+ SelectChannel(padapter, backup_oper_channel);
+ } else {
+ if (rx_chk != _SUCCESS) {
+ if (pmlmeext->retry == 0) {
+ issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+ issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+ issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+ }
+ }
+
+ if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) {
+ tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
+ }
+ }
+
+ if (rx_chk == _FAIL) {
+ pmlmeext->retry++;
+ if (pmlmeext->retry > rx_chk_limit) {
+ DBG_88E_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
+ FUNC_ADPT_ARG(padapter));
+ receive_disconnect(padapter, pmlmeinfo->network.MacAddress,
+ WLAN_REASON_EXPIRATION_CHK);
+ return;
+ }
+ } else {
+ pmlmeext->retry = 0;
+ }
+
+ if (tx_chk == _FAIL) {
+ pmlmeinfo->link_count &= 0xf;
+ } else {
+ pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
+ pmlmeinfo->link_count = 0;
+ }
+ } /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */
+ } else if (is_client_associated_to_ibss(padapter)) {
+ /* linked IBSS mode */
+ /* for each assoc list entry to check the rx pkt counter */
+ for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+ if (pmlmeinfo->FW_sta_info[i].status == 1) {
+ psta = pmlmeinfo->FW_sta_info[i].psta;
+
+ if (NULL == psta)
+ continue;
+ if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) {
+ if (pmlmeinfo->FW_sta_info[i].retry < 3) {
+ pmlmeinfo->FW_sta_info[i].retry++;
+ } else {
+ pmlmeinfo->FW_sta_info[i].retry = 0;
+ pmlmeinfo->FW_sta_info[i].status = 0;
+ report_del_sta_event(padapter, psta->hwaddr
+ , 65535/* indicate disconnect caused by no rx */
+ );
+ }
+ } else {
+ pmlmeinfo->FW_sta_info[i].retry = 0;
+ pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
+ }
+ }
+ }
+ }
+}
+
+void survey_timer_hdl(struct adapter *padapter)
+{
+ struct cmd_obj *ph2c;
+ struct sitesurvey_parm *psurveyPara;
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif
+
+ /* issue rtw_sitesurvey_cmd */
+ if (pmlmeext->sitesurvey_res.state > SCAN_START) {
+ if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
+ pmlmeext->sitesurvey_res.channel_idx++;
+
+ if (pmlmeext->scan_abort) {
+ #ifdef CONFIG_88EU_P2P
+ if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) {
+ rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+ pmlmeext->sitesurvey_res.channel_idx = 3;
+ DBG_88E("%s idx:%d, cnt:%u\n", __func__
+ , pmlmeext->sitesurvey_res.channel_idx
+ , pwdinfo->find_phase_state_exchange_cnt
+ );
+ } else
+ #endif
+ {
+ pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+ DBG_88E("%s idx:%d\n", __func__
+ , pmlmeext->sitesurvey_res.channel_idx
+ );
+ }
+
+ pmlmeext->scan_abort = false;/* reset */
+ }
+
+ ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (ph2c == NULL)
+ goto exit_survey_timer_hdl;
+
+ psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+ if (psurveyPara == NULL) {
+ kfree(ph2c);
+ goto exit_survey_timer_hdl;
+ }
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+ rtw_enqueue_cmd(pcmdpriv, ph2c);
+ }
+
+exit_survey_timer_hdl:
+ return;
+}
+
+void link_timer_hdl(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+ DBG_88E("link_timer_hdl:no beacon while connecting\n");
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ report_join_res(padapter, -3);
+ } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
+ /* re-auth timer */
+ if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
+ pmlmeinfo->state = 0;
+ report_join_res(padapter, -1);
+ return;
+ }
+
+ DBG_88E("link_timer_hdl: auth timeout and try again\n");
+ pmlmeinfo->auth_seq = 1;
+ issue_auth(padapter, NULL, 0);
+ set_link_timer(pmlmeext, REAUTH_TO);
+ } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
+ /* re-assoc timer */
+ if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+ report_join_res(padapter, -2);
+ return;
+ }
+
+ DBG_88E("link_timer_hdl: assoc timeout and try again\n");
+ issue_assocreq(padapter);
+ set_link_timer(pmlmeext, REASSOC_TO);
+ }
+ return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+ struct ht_priv *phtpriv;
+
+ if (!psta)
+ return;
+
+ phtpriv = &psta->htpriv;
+
+ if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+ if (phtpriv->candidate_tid_bitmap)
+ phtpriv->candidate_tid_bitmap = 0x0;
+ }
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ return H2C_SUCCESS;
+}
+
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ u8 type;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+ if (psetop->mode == Ndis802_11APMode) {
+ pmlmeinfo->state = WIFI_FW_AP_STATE;
+ type = _HW_STATE_AP_;
+ } else if (psetop->mode == Ndis802_11Infrastructure) {
+ pmlmeinfo->state &= ~(BIT(0)|BIT(1));/* clear state */
+ pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to STATION_STATE */
+ type = _HW_STATE_STATION_;
+ } else if (psetop->mode == Ndis802_11IBSS) {
+ type = _HW_STATE_ADHOC_;
+ } else {
+ type = _HW_STATE_NOLINK_;
+ }
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+ /* Set_NETYPE0_MSR(padapter, type); */
+
+ return H2C_SUCCESS;
+}
+
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+ struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+ /* u32 initialgain; */
+
+ if (pparm->network.InfrastructureMode == Ndis802_11APMode) {
+#ifdef CONFIG_88EU_AP_MODE
+
+ if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
+ /* todo: */
+ return H2C_SUCCESS;
+ }
+#endif
+ }
+
+ /* below is for ad-hoc master */
+ if (pparm->network.InfrastructureMode == Ndis802_11IBSS) {
+ rtw_joinbss_reset(padapter);
+
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ pmlmeinfo->ERP_enable = 0;
+ pmlmeinfo->WMM_enable = 0;
+ pmlmeinfo->HT_enable = 0;
+ pmlmeinfo->HT_caps_enable = 0;
+ pmlmeinfo->HT_info_enable = 0;
+ pmlmeinfo->agg_enable_bitmap = 0;
+ pmlmeinfo->candidate_tid_bitmap = 0;
+
+ /* disable dynamic functions, such as high power, DIG */
+ Save_DM_Func_Flag(padapter);
+ Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+ /* config the initial gain under linking, need to write the BB registers */
+ /* initialgain = 0x1E; */
+ /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+ /* cancel link timer */
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ /* clear CAM */
+ flush_all_cam_entry(padapter);
+
+ memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+ pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+ if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+ return H2C_PARAMETERS_ERROR;
+
+ memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+ start_create_ibss(padapter);
+ }
+
+ return H2C_SUCCESS;
+}
+
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ u8 join_type;
+ struct ndis_802_11_var_ie *pIE;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+ struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+ u32 i;
+
+ /* check already connecting to AP or not */
+ if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+ if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
+ issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 5, 100);
+
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+ /* clear CAM */
+ flush_all_cam_entry(padapter);
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ /* set MSR to nolink -> infra. mode */
+ Set_MSR(padapter, _HW_STATE_STATION_);
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+ }
+
+ rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, false);
+
+ rtw_joinbss_reset(padapter);
+
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ pmlmeinfo->ERP_enable = 0;
+ pmlmeinfo->WMM_enable = 0;
+ pmlmeinfo->HT_enable = 0;
+ pmlmeinfo->HT_caps_enable = 0;
+ pmlmeinfo->HT_info_enable = 0;
+ pmlmeinfo->agg_enable_bitmap = 0;
+ pmlmeinfo->candidate_tid_bitmap = 0;
+ pmlmeinfo->bwmode_updated = false;
+
+ memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+ pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+ if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+ return H2C_PARAMETERS_ERROR;
+
+ memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+ /* Check AP vendor to move rtw_joinbss_cmd() */
+
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < pnetwork->IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(pnetwork->IEs + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
+ if (!memcmp(pIE->data, WMM_OUI, 4))
+ pmlmeinfo->WMM_enable = 1;
+ break;
+ case _HT_CAPABILITY_IE_: /* Get HT Cap IE. */
+ pmlmeinfo->HT_caps_enable = 1;
+ break;
+ case _HT_EXTRA_INFO_IE_: /* Get HT Info IE. */
+ pmlmeinfo->HT_info_enable = 1;
+
+ /* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
+ {
+ struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
+
+ if ((pregpriv->cbw40_enable) && (pht_info->infos[0] & BIT(2))) {
+ /* switch to the 40M Hz mode according to the AP */
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+ switch (pht_info->infos[0] & 0x3) {
+ case 1:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+ break;
+ case 3:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+ break;
+ default:
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ break;
+ }
+
+ DBG_88E("set ch/bw before connected\n");
+ }
+ }
+ break;
+ default:
+ break;
+ }
+
+ i += (pIE->Length + 2);
+ }
+ /* disable dynamic functions, such as high power, DIG */
+
+ /* config the initial gain under linking, need to write the BB registers */
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+ join_type = 0;
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+ /* cancel link timer */
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ start_clnt_join(padapter);
+
+ return H2C_SUCCESS;
+}
+
+u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+ u8 val8;
+
+ if (is_client_associated_to_ap(padapter))
+ issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+ rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+ /* restore to initial setting. */
+ update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+ if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+ /* Stop BCN */
+ val8 = 0;
+ rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
+ }
+
+ /* set MSR to no link state -> infra. mode */
+ Set_MSR(padapter, _HW_STATE_STATION_);
+
+ pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+ /* switch to the 20M Hz mode after disconnect */
+ pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+ pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+ set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+ flush_all_cam_entry(padapter);
+
+ _cancel_timer_ex(&pmlmeext->link_timer);
+
+ rtw_free_uc_swdec_pending_queue(padapter);
+
+ return H2C_SUCCESS;
+}
+
+static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
+ u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+ int i, j;
+ int set_idx;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ /* clear out first */
+ memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
+
+ /* acquire channels from in */
+ j = 0;
+ for (i = 0; i < in_num; i++) {
+ set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+ if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED) &&
+ set_idx >= 0) {
+ memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
+
+ if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+ out[j].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+ j++;
+ }
+ if (j >= out_num)
+ break;
+ }
+
+ /* if out is empty, use channel_set as default */
+ if (j == 0) {
+ for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+ out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
+
+ if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+ out[i].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+ j++;
+ }
+ }
+
+ return j;
+}
+
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
+ u8 bdelayscan = false;
+ u8 val8;
+ u32 initialgain;
+ u32 i;
+
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+#endif
+
+ if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
+ /* for first time sitesurvey_cmd */
+ rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, NULL);
+
+ pmlmeext->sitesurvey_res.state = SCAN_START;
+ pmlmeext->sitesurvey_res.bss_cnt = 0;
+ pmlmeext->sitesurvey_res.channel_idx = 0;
+
+ for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+ if (pparm->ssid[i].SsidLength) {
+ memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
+ pmlmeext->sitesurvey_res.ssid[i].SsidLength = pparm->ssid[i].SsidLength;
+ } else {
+ pmlmeext->sitesurvey_res.ssid[i].SsidLength = 0;
+ }
+ }
+
+ pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
+ , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
+ , pparm->ch, pparm->ch_num
+ );
+
+ pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
+
+ /* issue null data if associating to the AP */
+ if (is_client_associated_to_ap(padapter)) {
+ pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+ issue_nulldata(padapter, NULL, 1, 3, 500);
+
+ bdelayscan = true;
+ }
+ if (bdelayscan) {
+ /* delay 50ms to protect nulldata(1). */
+ set_survey_timer(pmlmeext, 50);
+ return H2C_SUCCESS;
+ }
+ }
+
+ if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+ /* disable dynamic functions, such as high power, DIG */
+ Save_DM_Func_Flag(padapter);
+ Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+ /* config the initial gain under scanning, need to write the BB registers */
+#ifdef CONFIG_88EU_P2P
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ initialgain = 0x1E;
+ else
+ initialgain = 0x28;
+#else /* CONFIG_88EU_P2P */
+ initialgain = 0x1E;
+#endif /* CONFIG_88EU_P2P */
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+
+ /* set MSR to no link state */
+ Set_MSR(padapter, _HW_STATE_NOLINK_);
+
+ val8 = 1; /* under site survey */
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+ pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
+ }
+
+ site_survey(padapter);
+
+ return H2C_SUCCESS;
+}
+
+u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ struct setauth_parm *pparm = (struct setauth_parm *)pbuf;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pparm->mode < 4)
+ pmlmeinfo->auth_algo = pparm->mode;
+ return H2C_SUCCESS;
+}
+
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ unsigned short ctrl;
+ struct setkey_parm *pparm = (struct setkey_parm *)pbuf;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+ /* main tx key for wep. */
+ if (pparm->set_tx)
+ pmlmeinfo->key_index = pparm->keyid;
+
+ /* write cam */
+ ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+
+ DBG_88E_LEVEL(_drv_info_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
+ "keyid:%d\n", pparm->algorithm, pparm->keyid);
+ write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
+
+ return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ u16 ctrl = 0;
+ u8 cam_id;/* cam_entry */
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf;
+
+ /* cam_entry: */
+ /* 0~3 for default key */
+
+ /* for concurrent mode (ap+sta): */
+ /* default key is disable, using sw encrypt/decrypt */
+ /* cam_entry = 4 for sta mode (macid = 0) */
+ /* cam_entry(macid+3) = 5 ~ N for ap mode (aid = 1~N, macid = 2 ~N) */
+
+ /* for concurrent mode (sta+sta): */
+ /* default key is disable, using sw encrypt/decrypt */
+ /* cam_entry = 4 mapping to macid = 0 */
+ /* cam_entry = 5 mapping to macid = 2 */
+
+ cam_id = 4;
+
+ DBG_88E_LEVEL(_drv_info_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
+ pparm->algorithm, cam_id);
+ if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+ struct sta_info *psta;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ if (pparm->algorithm == _NO_PRIVACY_) /* clear cam entry */ {
+ clear_cam_entry(padapter, pparm->id);
+ return H2C_SUCCESS_RSP;
+ }
+
+ psta = rtw_get_stainfo(pstapriv, pparm->addr);
+ if (psta) {
+ ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+
+ DBG_88E("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
+
+ if ((psta->mac_id < 1) || (psta->mac_id > (NUM_STA-4))) {
+ DBG_88E("r871x_set_stakey_hdl():set_stakey failed, mac_id(aid)=%d\n", psta->mac_id);
+ return H2C_REJECTED;
+ }
+
+ cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+
+ DBG_88E("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0],
+ pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
+ pparm->addr[5], cam_id);
+
+ write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+
+ return H2C_SUCCESS_RSP;
+ } else {
+ DBG_88E("r871x_set_stakey_hdl(): sta has been free\n");
+ return H2C_REJECTED;
+ }
+ }
+
+ /* below for sta mode */
+
+ if (pparm->algorithm == _NO_PRIVACY_) { /* clear cam entry */
+ clear_cam_entry(padapter, pparm->id);
+ return H2C_SUCCESS;
+ }
+ ctrl = BIT(15) | ((pparm->algorithm) << 2);
+ write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+ pmlmeinfo->enc_algo = pparm->algorithm;
+ return H2C_SUCCESS;
+}
+
+u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+
+ if (!psta)
+ return H2C_SUCCESS;
+
+ if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+ ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+ issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+ _set_timer(&psta->addba_retry_timer, ADDBA_TO);
+ } else {
+ psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+ }
+ return H2C_SUCCESS;
+}
+
+u8 set_tx_beacon_cmd(struct adapter *padapter)
+{
+ struct cmd_obj *ph2c;
+ struct Tx_Beacon_param *ptxBeacon_parm;
+ struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ u8 res = _SUCCESS;
+ int len_diff = 0;
+
+ ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+ if (ptxBeacon_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+
+ len_diff = update_hidden_ssid(ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_,
+ ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_,
+ pmlmeinfo->hidden_ssid_mode);
+ ptxBeacon_parm->network.IELength += len_diff;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+ return res;
+}
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ u8 evt_code;
+ u16 evt_sz;
+ uint *peventbuf;
+ void (*event_callback)(struct adapter *dev, u8 *pbuf);
+ struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+ peventbuf = (uint *)pbuf;
+ evt_sz = (u16)(*peventbuf&0xffff);
+ evt_code = (u8)((*peventbuf>>16)&0xff);
+
+ /* checking if event code is valid */
+ if (evt_code >= MAX_C2HEVT) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
+ goto _abort_event_;
+ }
+
+ /* checking if event size match the event parm size */
+ if ((wlanevents[evt_code].parmsize != 0) &&
+ (wlanevents[evt_code].parmsize != evt_sz)) {
+ RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+ ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
+ evt_code, wlanevents[evt_code].parmsize, evt_sz));
+ goto _abort_event_;
+ }
+
+ ATOMIC_INC(&pevt_priv->event_seq);
+
+ peventbuf += 2;
+
+ if (peventbuf) {
+ event_callback = wlanevents[evt_code].event_callback;
+ event_callback(padapter, (u8 *)peventbuf);
+
+ pevt_priv->evt_done_cnt++;
+ }
+
+_abort_event_:
+ return H2C_SUCCESS;
+}
+
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ if (!pbuf)
+ return H2C_PARAMETERS_ERROR;
+
+ return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ if (send_beacon(padapter) == _FAIL) {
+ DBG_88E("issue_beacon, fail!\n");
+ return H2C_PARAMETERS_ERROR;
+ }
+#ifdef CONFIG_88EU_AP_MODE
+ else { /* tx bc/mc frames after update TIM */
+ struct sta_info *psta_bmc;
+ struct list_head *xmitframe_plist, *xmitframe_phead;
+ struct xmit_frame *pxmitframe = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ /* for BC/MC Frames */
+ psta_bmc = rtw_get_bcmc_stainfo(padapter);
+ if (!psta_bmc)
+ return H2C_SUCCESS;
+
+ if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len > 0)) {
+ rtw_msleep_os(10);/* 10ms, ATIM(HIQ) Windows */
+ spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+ xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+ xmitframe_plist = xmitframe_phead->next;
+
+ while (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ list_del_init(&pxmitframe->list);
+
+ psta_bmc->sleepq_len--;
+ if (psta_bmc->sleepq_len > 0)
+ pxmitframe->attrib.mdata = 1;
+ else
+ pxmitframe->attrib.mdata = 0;
+
+ pxmitframe->attrib.triggered = 1;
+
+ pxmitframe->attrib.qsel = 0x11;/* HIQ */
+
+ spin_unlock_bh(&psta_bmc->sleep_q.lock);
+ if (rtw_hal_xmit(padapter, pxmitframe))
+ rtw_os_xmit_complete(padapter, pxmitframe);
+ spin_lock_bh(&psta_bmc->sleep_q.lock);
+ }
+ spin_unlock_bh(&psta_bmc->sleep_q.lock);
+ }
+ }
+#endif
+ return H2C_SUCCESS;
+}
+
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
+{
+ struct set_ch_parm *set_ch_parm;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ if (!pbuf)
+ return H2C_PARAMETERS_ERROR;
+
+ set_ch_parm = (struct set_ch_parm *)pbuf;
+
+ DBG_88E(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+ FUNC_NDEV_ARG(padapter->pnetdev),
+ set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+
+ pmlmeext->cur_channel = set_ch_parm->ch;
+ pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+ pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+ set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+ return H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ struct SetChannelPlan_param *setChannelPlan_param;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ if (!pbuf)
+ return H2C_PARAMETERS_ERROR;
+
+ setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+ pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+ init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+ return H2C_SUCCESS;
+}
+
+u8 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ if (!pbuf)
+ return H2C_PARAMETERS_ERROR;
+ return H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ return H2C_REJECTED;
+}
+
+/* TDLS_WRCR : write RCR DATA BIT */
+/* TDLS_SD_PTI : issue peer traffic indication */
+/* TDLS_CS_OFF : go back to the channel linked with AP, terminating channel switch procedure */
+/* TDLS_INIT_CH_SEN : init channel sensing, receive all data and mgnt frame */
+/* TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/* TDLS_OFF_CH : first time set channel to off channel */
+/* TDLS_BASE_CH : go back tp the channel linked with AP when set base channel as target channel */
+/* TDLS_P_OFF_CH : periodically go to off channel */
+/* TDLS_P_BASE_CH : periodically go back to base channel */
+/* TDLS_RS_RCR : restore RCR */
+/* TDLS_CKALV_PH1 : check alive timer phase1 */
+/* TDLS_CKALV_PH2 : check alive timer phase2 */
+/* TDLS_FREE_STA : free tdls sta */
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+ return H2C_REJECTED;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mp.c b/drivers/staging/r8188eu/core/rtw_mp.c
new file mode 100644
index 000000000000..30bcac68f8e9
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mp.c
@@ -0,0 +1,1000 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ *published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MP_C_
+
+#include <drv_types.h>
+
+#include "odm_precomp.h"
+#include "rtl8188e_hal.h"
+
+u32 read_macreg(struct adapter *padapter, u32 addr, u32 sz)
+{
+ u32 val = 0;
+
+ switch (sz) {
+ case 1:
+ val = rtw_read8(padapter, addr);
+ break;
+ case 2:
+ val = rtw_read16(padapter, addr);
+ break;
+ case 4:
+ val = rtw_read32(padapter, addr);
+ break;
+ default:
+ val = 0xffffffff;
+ break;
+ }
+
+ return val;
+}
+
+void write_macreg(struct adapter *padapter, u32 addr, u32 val, u32 sz)
+{
+ switch (sz) {
+ case 1:
+ rtw_write8(padapter, addr, (u8)val);
+ break;
+ case 2:
+ rtw_write16(padapter, addr, (u16)val);
+ break;
+ case 4:
+ rtw_write32(padapter, addr, val);
+ break;
+ default:
+ break;
+ }
+}
+
+u32 read_bbreg(struct adapter *padapter, u32 addr, u32 bitmask)
+{
+ return rtw_hal_read_bbreg(padapter, addr, bitmask);
+}
+
+void write_bbreg(struct adapter *padapter, u32 addr, u32 bitmask, u32 val)
+{
+ rtw_hal_write_bbreg(padapter, addr, bitmask, val);
+}
+
+u32 _read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask)
+{
+ return rtw_hal_read_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bitmask);
+}
+
+void _write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val)
+{
+ rtw_hal_write_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bitmask, val);
+}
+
+u32 read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr)
+{
+ return _read_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bRFRegOffsetMask);
+}
+
+void write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 val)
+{
+ _write_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bRFRegOffsetMask, val);
+}
+
+static void _init_mp_priv_(struct mp_priv *pmp_priv)
+{
+ struct wlan_bssid_ex *pnetwork;
+
+ memset(pmp_priv, 0, sizeof(struct mp_priv));
+
+ pmp_priv->mode = MP_OFF;
+
+ pmp_priv->channel = 1;
+ pmp_priv->bandwidth = HT_CHANNEL_WIDTH_20;
+ pmp_priv->prime_channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ pmp_priv->rateidx = MPT_RATE_1M;
+ pmp_priv->txpoweridx = 0x2A;
+
+ pmp_priv->antenna_tx = ANTENNA_A;
+ pmp_priv->antenna_rx = ANTENNA_AB;
+
+ pmp_priv->check_mp_pkt = 0;
+
+ pmp_priv->tx_pktcount = 0;
+
+ pmp_priv->rx_pktcount = 0;
+ pmp_priv->rx_crcerrpktcount = 0;
+
+ pmp_priv->network_macaddr[0] = 0x00;
+ pmp_priv->network_macaddr[1] = 0xE0;
+ pmp_priv->network_macaddr[2] = 0x4C;
+ pmp_priv->network_macaddr[3] = 0x87;
+ pmp_priv->network_macaddr[4] = 0x66;
+ pmp_priv->network_macaddr[5] = 0x55;
+
+ pnetwork = &pmp_priv->mp_network.network;
+ memcpy(pnetwork->MacAddress, pmp_priv->network_macaddr, ETH_ALEN);
+
+ pnetwork->Ssid.SsidLength = 8;
+ memcpy(pnetwork->Ssid.Ssid, "mp_871x", pnetwork->Ssid.SsidLength);
+}
+
+static void mp_init_xmit_attrib(struct mp_tx *pmptx, struct adapter *padapter)
+{
+ struct pkt_attrib *pattrib;
+ struct tx_desc *desc;
+
+ /* init xmitframe attribute */
+ pattrib = &pmptx->attrib;
+ memset(pattrib, 0, sizeof(struct pkt_attrib));
+ desc = &pmptx->desc;
+ memset(desc, 0, TXDESC_SIZE);
+
+ pattrib->ether_type = 0x8712;
+ memset(pattrib->dst, 0xFF, ETH_ALEN);
+ pattrib->ack_policy = 0;
+ pattrib->hdrlen = WLAN_HDR_A3_LEN;
+ pattrib->subtype = WIFI_DATA;
+ pattrib->priority = 0;
+ pattrib->qsel = pattrib->priority;
+ pattrib->nr_frags = 1;
+ pattrib->encrypt = 0;
+ pattrib->bswenc = false;
+ pattrib->qos_en = false;
+}
+
+s32 init_mp_priv(struct adapter *padapter)
+{
+ struct mp_priv *pmppriv = &padapter->mppriv;
+
+ _init_mp_priv_(pmppriv);
+ pmppriv->papdater = padapter;
+
+ pmppriv->tx.stop = 1;
+ mp_init_xmit_attrib(&pmppriv->tx, padapter);
+
+ switch (padapter->registrypriv.rf_config) {
+ case RF_1T1R:
+ pmppriv->antenna_tx = ANTENNA_A;
+ pmppriv->antenna_rx = ANTENNA_A;
+ break;
+ case RF_1T2R:
+ default:
+ pmppriv->antenna_tx = ANTENNA_A;
+ pmppriv->antenna_rx = ANTENNA_AB;
+ break;
+ case RF_2T2R:
+ case RF_2T2R_GREEN:
+ pmppriv->antenna_tx = ANTENNA_AB;
+ pmppriv->antenna_rx = ANTENNA_AB;
+ break;
+ case RF_2T4R:
+ pmppriv->antenna_tx = ANTENNA_AB;
+ pmppriv->antenna_rx = ANTENNA_ABCD;
+ break;
+ }
+
+ return _SUCCESS;
+}
+
+void free_mp_priv(struct mp_priv *pmp_priv)
+{
+ kfree(pmp_priv->pallocated_mp_xmitframe_buf);
+ pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+ pmp_priv->pmp_xmtframe_buf = NULL;
+}
+
+#define PHY_IQCalibrate(a, b) PHY_IQCalibrate_8188E(a, b)
+#define PHY_LCCalibrate(a) PHY_LCCalibrate_8188E(a)
+#define PHY_SetRFPathSwitch(a, b) PHY_SetRFPathSwitch_8188E(a, b)
+
+s32 MPT_InitializeAdapter(struct adapter *pAdapter, u8 Channel)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(pAdapter);
+ s32 rtStatus = _SUCCESS;
+ struct mpt_context *pMptCtx = &pAdapter->mppriv.MptCtx;
+ struct mlme_priv *pmlmepriv = &pAdapter->mlmepriv;
+
+ /* HW Initialization for 8190 MPT. */
+ /* SW Initialization for 8190 MP. */
+ pMptCtx->bMptDrvUnload = false;
+ pMptCtx->bMassProdTest = false;
+ pMptCtx->bMptIndexEven = true; /* default gain index is -6.0db */
+ pMptCtx->h2cReqNum = 0x0;
+ /* Init mpt event. */
+ /* init for BT MP */
+
+ pMptCtx->bMptWorkItemInProgress = false;
+ pMptCtx->CurrMptAct = NULL;
+ /* */
+
+ /* Don't accept any packets */
+ rtw_write32(pAdapter, REG_RCR, 0);
+
+ PHY_IQCalibrate(pAdapter, false);
+ dm_CheckTXPowerTracking(&pHalData->odmpriv); /* trigger thermal meter */
+ PHY_LCCalibrate(pAdapter);
+
+ pMptCtx->backup0xc50 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0);
+ pMptCtx->backup0xc58 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0);
+ pMptCtx->backup0xc30 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_RxDetector1, bMaskByte0);
+ pMptCtx->backup0x52_RF_A = (u8)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+ pMptCtx->backup0x52_RF_B = (u8)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+
+ /* set ant to wifi side in mp mode */
+ rtw_write16(pAdapter, 0x870, 0x300);
+ rtw_write16(pAdapter, 0x860, 0x110);
+
+ if (pAdapter->registrypriv.mp_mode == 1)
+ pmlmepriv->fw_state = WIFI_MP_STATE;
+
+ return rtStatus;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function: MPT_DeInitAdapter()
+ *
+ * Overview: Extra DeInitialization for Mass Production Test.
+ *
+ * Input: struct adapter * pAdapter
+ *
+ * Output: NONE
+ *
+ * Return: NONE
+ *
+ * Revised History:
+ * When Who Remark
+ * 05/08/2007 MHC Create Version 0.
+ * 05/18/2007 MHC Add normal driver MPHalt code.
+ *
+ *---------------------------------------------------------------------------*/
+void MPT_DeInitAdapter(struct adapter *pAdapter)
+{
+ struct mpt_context *pMptCtx = &pAdapter->mppriv.MptCtx;
+
+ pMptCtx->bMptDrvUnload = true;
+}
+
+static u8 mpt_ProStartTest(struct adapter *padapter)
+{
+ struct mpt_context *pMptCtx = &padapter->mppriv.MptCtx;
+
+ pMptCtx->bMassProdTest = true;
+ pMptCtx->bStartContTx = false;
+ pMptCtx->bCckContTx = false;
+ pMptCtx->bOfdmContTx = false;
+ pMptCtx->bSingleCarrier = false;
+ pMptCtx->bCarrierSuppression = false;
+ pMptCtx->bSingleTone = false;
+
+ return _SUCCESS;
+}
+
+/*
+ * General use
+ */
+s32 SetPowerTracking(struct adapter *padapter, u8 enable)
+{
+ Hal_SetPowerTracking(padapter, enable);
+ return 0;
+}
+
+void GetPowerTracking(struct adapter *padapter, u8 *enable)
+{
+ Hal_GetPowerTracking(padapter, enable);
+}
+
+static void disable_dm(struct adapter *padapter)
+{
+ u8 v8;
+
+ /* 3 1. disable firmware dynamic mechanism */
+ /* disable Power Training, Rate Adaptive */
+ v8 = rtw_read8(padapter, REG_BCN_CTRL);
+ v8 &= ~EN_BCN_FUNCTION;
+ rtw_write8(padapter, REG_BCN_CTRL, v8);
+
+ /* 3 2. disable driver dynamic mechanism */
+ /* disable Dynamic Initial Gain */
+ /* disable High Power */
+ /* disable Power Tracking */
+ Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+ /* enable APK, LCK and IQK but disable power tracking */
+ Switch_DM_Func(padapter, DYNAMIC_RF_CALIBRATION, true);
+}
+
+/* This function initializes the DUT to the MP test mode */
+s32 mp_start_test(struct adapter *padapter)
+{
+ struct wlan_bssid_ex bssid;
+ struct sta_info *psta;
+ u32 length;
+ u8 val8;
+ s32 res = _SUCCESS;
+ struct mp_priv *pmppriv = &padapter->mppriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+ padapter->registrypriv.mp_mode = 1;
+ pmppriv->bSetTxPower = 0; /* for manually set tx power */
+
+ /* 3 disable dynamic mechanism */
+ disable_dm(padapter);
+
+ /* 3 0. update mp_priv */
+
+ if (padapter->registrypriv.rf_config == RF_819X_MAX_TYPE) {
+ switch (GET_RF_TYPE(padapter)) {
+ case RF_1T1R:
+ pmppriv->antenna_tx = ANTENNA_A;
+ pmppriv->antenna_rx = ANTENNA_A;
+ break;
+ case RF_1T2R:
+ default:
+ pmppriv->antenna_tx = ANTENNA_A;
+ pmppriv->antenna_rx = ANTENNA_AB;
+ break;
+ case RF_2T2R:
+ case RF_2T2R_GREEN:
+ pmppriv->antenna_tx = ANTENNA_AB;
+ pmppriv->antenna_rx = ANTENNA_AB;
+ break;
+ case RF_2T4R:
+ pmppriv->antenna_tx = ANTENNA_AB;
+ pmppriv->antenna_rx = ANTENNA_ABCD;
+ break;
+ }
+ }
+
+ mpt_ProStartTest(padapter);
+
+ /* 3 1. initialize a new struct wlan_bssid_ex */
+/* memset(&bssid, 0, sizeof(struct wlan_bssid_ex)); */
+ memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN);
+ bssid.Ssid.SsidLength = strlen("mp_pseudo_adhoc");
+ memcpy(bssid.Ssid.Ssid, (u8 *)"mp_pseudo_adhoc", bssid.Ssid.SsidLength);
+ bssid.InfrastructureMode = Ndis802_11IBSS;
+ bssid.NetworkTypeInUse = Ndis802_11DS;
+ bssid.IELength = 0;
+
+ length = get_wlan_bssid_ex_sz(&bssid);
+ if (length % 4)
+ bssid.Length = ((length >> 2) + 1) << 2; /* round up to multiple of 4 bytes. */
+ else
+ bssid.Length = length;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)
+ goto end_of_mp_start_test;
+
+ /* init mp_start_test status */
+ if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+ rtw_disassoc_cmd(padapter, 500, true);
+ rtw_indicate_disconnect(padapter);
+ rtw_free_assoc_resources(padapter, 1);
+ }
+ pmppriv->prev_fw_state = get_fwstate(pmlmepriv);
+ if (padapter->registrypriv.mp_mode == 1)
+ pmlmepriv->fw_state = WIFI_MP_STATE;
+ set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+ /* 3 2. create a new psta for mp driver */
+ /* clear psta in the cur_network, if any */
+ psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
+ if (psta)
+ rtw_free_stainfo(padapter, psta);
+
+ psta = rtw_alloc_stainfo(&padapter->stapriv, bssid.MacAddress);
+ if (psta == NULL) {
+ RT_TRACE(_module_mp_, _drv_err_, ("mp_start_test: Can't alloc sta_info!\n"));
+ pmlmepriv->fw_state = pmppriv->prev_fw_state;
+ res = _FAIL;
+ goto end_of_mp_start_test;
+ }
+
+ /* 3 3. join psudo AdHoc */
+ tgt_network->join_res = 1;
+ tgt_network->aid = 1;
+ psta->aid = 1;
+ memcpy(&tgt_network->network, &bssid, length);
+
+ rtw_indicate_connect(padapter);
+ _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+end_of_mp_start_test:
+
+ spin_unlock_bh(&pmlmepriv->lock);
+
+ if (res == _SUCCESS) {
+ /* set MSR to WIFI_FW_ADHOC_STATE */
+ val8 = rtw_read8(padapter, MSR) & 0xFC; /* 0x0102 */
+ val8 |= WIFI_FW_ADHOC_STATE;
+ rtw_write8(padapter, MSR, val8); /* Link in ad hoc network */
+ }
+ return res;
+}
+/* */
+/* This function change the DUT from the MP test mode into normal mode */
+void mp_stop_test(struct adapter *padapter)
+{
+ struct mp_priv *pmppriv = &padapter->mppriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+ struct sta_info *psta;
+
+ if (pmppriv->mode == MP_ON) {
+ pmppriv->bSetTxPower = 0;
+ spin_lock_bh(&pmlmepriv->lock);
+ if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)
+ goto end_of_mp_stop_test;
+
+ /* 3 1. disconnect psudo AdHoc */
+ rtw_indicate_disconnect(padapter);
+
+ /* 3 2. clear psta used in mp test mode. */
+ psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
+ if (psta)
+ rtw_free_stainfo(padapter, psta);
+
+ /* 3 3. return to normal state (default:station mode) */
+ pmlmepriv->fw_state = pmppriv->prev_fw_state; /* WIFI_STATION_STATE; */
+
+ /* flush the cur_network */
+ memset(tgt_network, 0, sizeof(struct wlan_network));
+
+ _clr_fwstate_(pmlmepriv, WIFI_MP_STATE);
+
+end_of_mp_stop_test:
+
+ spin_unlock_bh(&pmlmepriv->lock);
+ }
+}
+
+/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
+/*
+ * SetChannel
+ * Description
+ * Use H2C command to change channel,
+ * not only modify rf register, but also other setting need to be done.
+ */
+void SetChannel(struct adapter *pAdapter)
+{
+ Hal_SetChannel(pAdapter);
+}
+
+/*
+ * Notice
+ * Switch bandwitdth may change center frequency(channel)
+ */
+void SetBandwidth(struct adapter *pAdapter)
+{
+ Hal_SetBandwidth(pAdapter);
+}
+
+void SetAntenna(struct adapter *pAdapter)
+{
+ Hal_SetAntenna(pAdapter);
+}
+
+void SetAntennaPathPower(struct adapter *pAdapter)
+{
+ Hal_SetAntennaPathPower(pAdapter);
+}
+
+void SetTxPower(struct adapter *pAdapter)
+{
+ Hal_SetTxPower(pAdapter);
+ }
+
+void SetDataRate(struct adapter *pAdapter)
+{
+ Hal_SetDataRate(pAdapter);
+}
+
+void MP_PHY_SetRFPathSwitch(struct adapter *pAdapter , bool bMain)
+{
+ PHY_SetRFPathSwitch(pAdapter, bMain);
+}
+
+s32 SetThermalMeter(struct adapter *pAdapter, u8 target_ther)
+{
+ return Hal_SetThermalMeter(pAdapter, target_ther);
+}
+
+void GetThermalMeter(struct adapter *pAdapter, u8 *value)
+{
+ Hal_GetThermalMeter(pAdapter, value);
+}
+
+void SetSingleCarrierTx(struct adapter *pAdapter, u8 bStart)
+{
+ PhySetTxPowerLevel(pAdapter);
+ Hal_SetSingleCarrierTx(pAdapter, bStart);
+}
+
+void SetSingleToneTx(struct adapter *pAdapter, u8 bStart)
+{
+ PhySetTxPowerLevel(pAdapter);
+ Hal_SetSingleToneTx(pAdapter, bStart);
+}
+
+void SetCarrierSuppressionTx(struct adapter *pAdapter, u8 bStart)
+{
+ PhySetTxPowerLevel(pAdapter);
+ Hal_SetCarrierSuppressionTx(pAdapter, bStart);
+}
+
+void SetContinuousTx(struct adapter *pAdapter, u8 bStart)
+{
+ PhySetTxPowerLevel(pAdapter);
+ Hal_SetContinuousTx(pAdapter, bStart);
+}
+
+void PhySetTxPowerLevel(struct adapter *pAdapter)
+{
+ struct mp_priv *pmp_priv = &pAdapter->mppriv;
+
+ if (pmp_priv->bSetTxPower == 0) /* for NO manually set power index */
+ PHY_SetTxPowerLevel8188E(pAdapter, pmp_priv->channel);
+}
+
+/* */
+static void dump_mpframe(struct adapter *padapter, struct xmit_frame *pmpframe)
+{
+ rtw_hal_mgnt_xmit(padapter, pmpframe);
+}
+
+static struct xmit_frame *alloc_mp_xmitframe(struct xmit_priv *pxmitpriv)
+{
+ struct xmit_frame *pmpframe;
+ struct xmit_buf *pxmitbuf;
+
+ pmpframe = rtw_alloc_xmitframe(pxmitpriv);
+ if (pmpframe == NULL)
+ return NULL;
+
+ pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+ if (pxmitbuf == NULL) {
+ rtw_free_xmitframe(pxmitpriv, pmpframe);
+ return NULL;
+ }
+
+ pmpframe->frame_tag = MP_FRAMETAG;
+
+ pmpframe->pxmitbuf = pxmitbuf;
+
+ pmpframe->buf_addr = pxmitbuf->pbuf;
+
+ pxmitbuf->priv_data = pmpframe;
+
+ return pmpframe;
+}
+
+static int mp_xmit_packet_thread(void *context)
+{
+ struct xmit_frame *pxmitframe;
+ struct mp_tx *pmptx;
+ struct mp_priv *pmp_priv;
+ struct xmit_priv *pxmitpriv;
+ struct adapter *padapter;
+
+ pmp_priv = (struct mp_priv *)context;
+ pmptx = &pmp_priv->tx;
+ padapter = pmp_priv->papdater;
+ pxmitpriv = &(padapter->xmitpriv);
+
+ thread_enter("RTW_MP_THREAD");
+
+ /* DBG_88E("%s:pkTx Start\n", __func__); */
+ while (1) {
+ pxmitframe = alloc_mp_xmitframe(pxmitpriv);
+ if (pxmitframe == NULL) {
+ if (pmptx->stop ||
+ padapter->bSurpriseRemoved ||
+ padapter->bDriverStopped) {
+ goto exit;
+ } else {
+ rtw_msleep_os(1);
+ continue;
+ }
+ }
+
+ memcpy((u8 *)(pxmitframe->buf_addr+TXDESC_OFFSET), pmptx->buf, pmptx->write_size);
+ memcpy(&(pxmitframe->attrib), &(pmptx->attrib), sizeof(struct pkt_attrib));
+
+ dump_mpframe(padapter, pxmitframe);
+
+ pmptx->sended++;
+ pmp_priv->tx_pktcount++;
+
+ if (pmptx->stop ||
+ padapter->bSurpriseRemoved ||
+ padapter->bDriverStopped)
+ goto exit;
+ if ((pmptx->count != 0) &&
+ (pmptx->count == pmptx->sended))
+ goto exit;
+
+ flush_signals_thread();
+ }
+
+exit:
+ kfree(pmptx->pallocated_buf);
+ pmptx->pallocated_buf = NULL;
+ pmptx->stop = 1;
+
+ thread_exit();
+}
+
+void fill_txdesc_for_mp(struct adapter *padapter, struct tx_desc *ptxdesc)
+{
+ struct mp_priv *pmp_priv = &padapter->mppriv;
+ memcpy(ptxdesc, &(pmp_priv->tx.desc), TXDESC_SIZE);
+}
+
+void SetPacketTx(struct adapter *padapter)
+{
+ u8 *ptr, *pkt_start, *pkt_end;
+ u32 pkt_size;
+ struct tx_desc *desc;
+ struct rtw_ieee80211_hdr *hdr;
+ u8 payload;
+ s32 bmcast;
+ struct pkt_attrib *pattrib;
+ struct mp_priv *pmp_priv;
+
+ pmp_priv = &padapter->mppriv;
+ if (pmp_priv->tx.stop)
+ return;
+ pmp_priv->tx.sended = 0;
+ pmp_priv->tx.stop = 0;
+ pmp_priv->tx_pktcount = 0;
+
+ /* 3 1. update_attrib() */
+ pattrib = &pmp_priv->tx.attrib;
+ memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
+ memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+ memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+ bmcast = IS_MCAST(pattrib->ra);
+ if (bmcast) {
+ pattrib->mac_id = 1;
+ pattrib->psta = rtw_get_bcmc_stainfo(padapter);
+ } else {
+ pattrib->mac_id = 0;
+ pattrib->psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+ }
+
+ pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->pktlen;
+
+ /* 3 2. allocate xmit buffer */
+ pkt_size = pattrib->last_txcmdsz;
+
+ kfree(pmp_priv->tx.pallocated_buf);
+ pmp_priv->tx.write_size = pkt_size;
+ pmp_priv->tx.buf_size = pkt_size + XMITBUF_ALIGN_SZ;
+ pmp_priv->tx.pallocated_buf = rtw_zmalloc(pmp_priv->tx.buf_size);
+ if (pmp_priv->tx.pallocated_buf == NULL) {
+ DBG_88E("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size);
+ return;
+ }
+ pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((size_t)(pmp_priv->tx.pallocated_buf), XMITBUF_ALIGN_SZ);
+ ptr = pmp_priv->tx.buf;
+
+ desc = &(pmp_priv->tx.desc);
+ memset(desc, 0, TXDESC_SIZE);
+ pkt_start = ptr;
+ pkt_end = pkt_start + pkt_size;
+
+ /* 3 3. init TX descriptor */
+ /* offset 0 */
+ desc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+ desc->txdw0 |= cpu_to_le32(pkt_size & 0x0000FFFF); /* packet size */
+ desc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00FF0000); /* 32 bytes for TX Desc */
+ if (bmcast)
+ desc->txdw0 |= cpu_to_le32(BMC); /* broadcast packet */
+
+ desc->txdw1 |= cpu_to_le32((0x01 << 26) & 0xff000000);
+ /* offset 4 */
+ desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x3F); /* CAM_ID(MAC_ID) */
+ desc->txdw1 |= cpu_to_le32((pattrib->qsel << QSEL_SHT) & 0x00001F00); /* Queue Select, TID */
+
+ desc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000F0000); /* Rate Adaptive ID */
+ /* offset 8 */
+ /* offset 12 */
+
+ desc->txdw3 |= cpu_to_le32((pattrib->seqnum<<16)&0x0fff0000);
+
+ /* offset 16 */
+ desc->txdw4 |= cpu_to_le32(HW_SSN);
+ desc->txdw4 |= cpu_to_le32(USERATE);
+ desc->txdw4 |= cpu_to_le32(DISDATAFB);
+
+ if (pmp_priv->preamble) {
+ if (pmp_priv->rateidx <= MPT_RATE_54M)
+ desc->txdw4 |= cpu_to_le32(DATA_SHORT); /* CCK Short Preamble */
+ }
+ if (pmp_priv->bandwidth == HT_CHANNEL_WIDTH_40)
+ desc->txdw4 |= cpu_to_le32(DATA_BW);
+
+ /* offset 20 */
+ desc->txdw5 |= cpu_to_le32(pmp_priv->rateidx & 0x0000001F);
+
+ if (pmp_priv->preamble) {
+ if (pmp_priv->rateidx > MPT_RATE_54M)
+ desc->txdw5 |= cpu_to_le32(SGI); /* MCS Short Guard Interval */
+ }
+ desc->txdw5 |= cpu_to_le32(RTY_LMT_EN); /* retry limit enable */
+ desc->txdw5 |= cpu_to_le32(0x00180000); /* DATA/RTS Rate Fallback Limit */
+
+ /* 3 4. make wlan header, make_wlanhdr() */
+ hdr = (struct rtw_ieee80211_hdr *)pkt_start;
+ SetFrameSubType(&hdr->frame_ctl, pattrib->subtype);
+ memcpy(hdr->addr1, pattrib->dst, ETH_ALEN); /* DA */
+ memcpy(hdr->addr2, pattrib->src, ETH_ALEN); /* SA */
+ memcpy(hdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN); /* RA, BSSID */
+
+ /* 3 5. make payload */
+ ptr = pkt_start + pattrib->hdrlen;
+
+ switch (pmp_priv->tx.payload) {
+ case 0:
+ payload = 0x00;
+ break;
+ case 1:
+ payload = 0x5a;
+ break;
+ case 2:
+ payload = 0xa5;
+ break;
+ case 3:
+ payload = 0xff;
+ break;
+ default:
+ payload = 0x00;
+ break;
+ }
+
+ memset(ptr, payload, pkt_end - ptr);
+
+ /* 3 6. start thread */
+ pmp_priv->tx.PktTxThread = kthread_run(mp_xmit_packet_thread, pmp_priv, "RTW_MP_THREAD");
+ if (IS_ERR(pmp_priv->tx.PktTxThread))
+ DBG_88E("Create PktTx Thread Fail !!!!!\n");
+}
+
+void SetPacketRx(struct adapter *pAdapter, u8 bStartRx)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(pAdapter);
+
+ if (bStartRx) {
+ /* Accept CRC error and destination address */
+ pHalData->ReceiveConfig = AAP | APM | AM | AB | APP_ICV |
+ AMF | ADF | APP_FCS | HTC_LOC_CTRL |
+ APP_MIC | APP_PHYSTS;
+
+ pHalData->ReceiveConfig |= (RCR_ACRC32 | RCR_AAP);
+
+ rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
+
+ /* Accept all data frames */
+ rtw_write16(pAdapter, REG_RXFLTMAP2, 0xFFFF);
+ } else {
+ rtw_write32(pAdapter, REG_RCR, 0);
+ }
+}
+
+void ResetPhyRxPktCount(struct adapter *pAdapter)
+{
+ u32 i, phyrx_set = 0;
+
+ for (i = 0; i <= 0xF; i++) {
+ phyrx_set = 0;
+ phyrx_set |= _RXERR_RPT_SEL(i); /* select */
+ phyrx_set |= RXERR_RPT_RST; /* set counter to zero */
+ rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+ }
+}
+
+static u32 GetPhyRxPktCounts(struct adapter *pAdapter, u32 selbit)
+{
+ /* selection */
+ u32 phyrx_set = 0, count = 0;
+
+ phyrx_set = _RXERR_RPT_SEL(selbit & 0xF);
+ rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+
+ /* Read packet count */
+ count = rtw_read32(pAdapter, REG_RXERR_RPT) & RXERR_COUNTER_MASK;
+
+ return count;
+}
+
+u32 GetPhyRxPktReceived(struct adapter *pAdapter)
+{
+ u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+ OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_OK);
+ CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_OK);
+ HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_OK);
+
+ return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+u32 GetPhyRxPktCRC32Error(struct adapter *pAdapter)
+{
+ u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+ OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_FAIL);
+ CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_FAIL);
+ HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_FAIL);
+
+ return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+/* reg 0x808[9:0]: FFT data x */
+/* reg 0x808[22]: 0 --> 1 to get 1 FFT data y */
+/* reg 0x8B4[15:0]: FFT data y report */
+static u32 rtw_GetPSDData(struct adapter *pAdapter, u32 point)
+{
+ int psd_val;
+
+ psd_val = rtw_read32(pAdapter, 0x808);
+ psd_val &= 0xFFBFFC00;
+ psd_val |= point;
+
+ rtw_write32(pAdapter, 0x808, psd_val);
+ rtw_mdelay_os(1);
+ psd_val |= 0x00400000;
+
+ rtw_write32(pAdapter, 0x808, psd_val);
+ rtw_mdelay_os(1);
+ psd_val = rtw_read32(pAdapter, 0x8B4);
+
+ psd_val &= 0x0000FFFF;
+
+ return psd_val;
+}
+
+/*
+ *pts start_point_min stop_point_max
+ * 128 64 64 + 128 = 192
+ * 256 128 128 + 256 = 384
+ * 512 256 256 + 512 = 768
+ * 1024 512 512 + 1024 = 1536
+ */
+u32 mp_query_psd(struct adapter *pAdapter, u8 *data)
+{
+ u32 i, psd_pts = 0, psd_start = 0, psd_stop = 0;
+ u32 psd_data = 0;
+
+ if (!netif_running(pAdapter->pnetdev)) {
+ RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! interface not opened!\n"));
+ return 0;
+ }
+
+ if (check_fwstate(&pAdapter->mlmepriv, WIFI_MP_STATE) == false) {
+ RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! not in MP mode!\n"));
+ return 0;
+ }
+
+ if (strlen(data) == 0) { /* default value */
+ psd_pts = 128;
+ psd_start = 64;
+ psd_stop = 128;
+ } else {
+ sscanf(data, "pts =%d, start =%d, stop =%d", &psd_pts, &psd_start, &psd_stop);
+ }
+
+ memset(data, '\0', sizeof(*data));
+
+ i = psd_start;
+ while (i < psd_stop) {
+ if (i >= psd_pts) {
+ psd_data = rtw_GetPSDData(pAdapter, i-psd_pts);
+ } else {
+ psd_data = rtw_GetPSDData(pAdapter, i);
+ }
+ sprintf(data + strlen(data), "%x ", psd_data);
+ i++;
+ }
+
+ rtw_msleep_os(100);
+ return strlen(data)+1;
+}
+
+void _rtw_mp_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+ int i, res;
+ struct adapter *padapter = pxmitpriv->adapter;
+ struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+ u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+ u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+ if (padapter->registrypriv.mp_mode == 0) {
+ max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+ num_xmit_extbuf = NR_XMIT_EXTBUFF;
+ } else {
+ max_xmit_extbuf_size = 6000;
+ num_xmit_extbuf = 8;
+ }
+
+ pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+ for (i = 0; i < num_xmit_extbuf; i++) {
+ rtw_os_xmit_resource_free(padapter, pxmitbuf, (max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+
+ pxmitbuf++;
+ }
+
+ if (pxmitpriv->pallocated_xmit_extbuf)
+ rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+
+ if (padapter->registrypriv.mp_mode == 0) {
+ max_xmit_extbuf_size = 6000;
+ num_xmit_extbuf = 8;
+ } else {
+ max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+ num_xmit_extbuf = NR_XMIT_EXTBUFF;
+ }
+
+ /* Init xmit extension buff */
+ _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+ pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+
+ if (pxmitpriv->pallocated_xmit_extbuf == NULL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n"));
+ res = _FAIL;
+ goto exit;
+ }
+
+ pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((size_t)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+ pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+ for (i = 0; i < num_xmit_extbuf; i++) {
+ INIT_LIST_HEAD(&pxmitbuf->list);
+
+ pxmitbuf->priv_data = NULL;
+ pxmitbuf->padapter = padapter;
+ pxmitbuf->ext_tag = true;
+
+ res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, max_xmit_extbuf_size + XMITBUF_ALIGN_SZ);
+ if (res == _FAIL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+ pxmitbuf++;
+ }
+
+ pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+exit:
+ ;
+}
+
+void Hal_ProSetCrystalCap (struct adapter *pAdapter, u32 CrystalCapVal)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(pAdapter);
+
+ CrystalCapVal = CrystalCapVal & 0x3F;
+
+ // write 0x24[16:11] = 0x24[22:17] = CrystalCap
+ PHY_SetBBReg(pAdapter, REG_AFE_XTAL_CTRL, 0x7FF800,
+ (CrystalCapVal | (CrystalCapVal << 6)));
+}
diff --git a/drivers/staging/r8188eu/core/rtw_mp_ioctl.c b/drivers/staging/r8188eu/core/rtw_mp_ioctl.c
new file mode 100644
index 000000000000..87c3f29b16ef
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_mp_ioctl.c
@@ -0,0 +1,1352 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MP_IOCTL_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <mlme_osdep.h>
+
+/* include <rtw_mp.h> */
+#include <rtw_mp_ioctl.h>
+
+/* rtl8188eu_oid_rtl_seg_81_85 section start **************** */
+int rtl8188eu_oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->information_buf_len < sizeof(u8))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ if (poid_par_priv->type_of_oid == SET_OID) {
+ Adapter->registrypriv.wireless_mode = *(u8 *)poid_par_priv->information_buf;
+ } else if (poid_par_priv->type_of_oid == QUERY_OID) {
+ *(u8 *)poid_par_priv->information_buf = Adapter->registrypriv.wireless_mode;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ RT_TRACE(_module_mp_, _drv_info_, ("-query Wireless Mode=%d\n", Adapter->registrypriv.wireless_mode));
+ } else {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ }
+
+ return status;
+}
+/* rtl8188eu_oid_rtl_seg_81_87_80 section start **************** */
+int rtl8188eu_oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct bb_reg_param *pbbreg;
+ u16 offset;
+ u32 value;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_write_bb_reg_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+
+ offset = (u16)(pbbreg->offset) & 0xFFF; /* 0ffset :0x800~0xfff */
+ if (offset < BB_REG_BASE_ADDR)
+ offset |= BB_REG_BASE_ADDR;
+
+ value = pbbreg->value;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_write_bb_reg_hdl: offset=0x%03X value=0x%08X\n",
+ offset, value));
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ write_bbreg(Adapter, offset, 0xFFFFFFFF, value);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct bb_reg_param *pbbreg;
+ u16 offset;
+ u32 value;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_read_bb_reg_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+
+ offset = (u16)(pbbreg->offset) & 0xFFF; /* 0ffset :0x800~0xfff */
+ if (offset < BB_REG_BASE_ADDR)
+ offset |= BB_REG_BASE_ADDR;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ value = read_bbreg(Adapter, offset, 0xFFFFFFFF);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ pbbreg->value = value;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("-rtl8188eu_oid_rt_pro_read_bb_reg_hdl: offset=0x%03X value:0x%08X\n",
+ offset, value));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct rf_reg_param *pbbreg;
+ u8 path;
+ u8 offset;
+ u32 value;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_write_rf_reg_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+
+ if (pbbreg->path >= RF_PATH_MAX)
+ return NDIS_STATUS_NOT_ACCEPTED;
+ if (pbbreg->offset > 0xFF)
+ return NDIS_STATUS_NOT_ACCEPTED;
+ if (pbbreg->value > 0xFFFFF)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ path = (u8)pbbreg->path;
+ offset = (u8)pbbreg->offset;
+ value = pbbreg->value;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_write_rf_reg_hdl: path=%d offset=0x%02X value=0x%05X\n",
+ path, offset, value));
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ write_rfreg(Adapter, path, offset, value);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct rf_reg_param *pbbreg;
+ u8 path;
+ u8 offset;
+ u32 value;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+ int status = NDIS_STATUS_SUCCESS;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_read_rf_reg_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+
+ if (pbbreg->path >= RF_PATH_MAX)
+ return NDIS_STATUS_NOT_ACCEPTED;
+ if (pbbreg->offset > 0xFF)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ path = (u8)pbbreg->path;
+ offset = (u8)pbbreg->offset;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ value = read_rfreg(Adapter, path, offset);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ pbbreg->value = value;
+
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("-rtl8188eu_oid_rt_pro_read_rf_reg_hdl: path=%d offset=0x%02X value=0x%05X\n",
+ path, offset, value));
+
+ return status;
+}
+/* rtl8188eu_oid_rtl_seg_81_87_00 section end**************** */
+/* */
+
+/* rtl8188eu_oid_rtl_seg_81_80_00 section start **************** */
+/* */
+int rtl8188eu_oid_rt_pro_set_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 ratevalue;/* 4 */
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("+rtl8188eu_oid_rt_pro_set_data_rate_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ ratevalue = *((u32 *)poid_par_priv->information_buf);/* 4 */
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_set_data_rate_hdl: data rate idx=%d\n", ratevalue));
+ if (ratevalue >= MPT_RATE_LAST)
+ return NDIS_STATUS_INVALID_DATA;
+
+ Adapter->mppriv.rateidx = ratevalue;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetDataRate(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 mode;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_start_test_hdl\n"));
+
+ if (Adapter->registrypriv.mp_mode == 0)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ /* IQCalibrateBcut(Adapter); */
+
+ mode = *((u32 *)poid_par_priv->information_buf);
+ Adapter->mppriv.mode = mode;/* 1 for loopback */
+
+ if (mp_start_test(Adapter) == _FAIL) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ goto exit;
+ }
+
+exit:
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("-rtl8188eu_oid_rt_pro_start_test_hdl: mp_mode=%d\n", Adapter->mppriv.mode));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_stop_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+Set OID_RT_PRO_STOP_TEST\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ mp_stop_test(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("-Set OID_RT_PRO_STOP_TEST\n"));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 Channel;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_set_channel_direct_call_hdl\n"));
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ if (poid_par_priv->type_of_oid == QUERY_OID) {
+ *((u32 *)poid_par_priv->information_buf) = Adapter->mppriv.channel;
+ return NDIS_STATUS_SUCCESS;
+ }
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ Channel = *((u32 *)poid_par_priv->information_buf);
+ RT_TRACE(_module_mp_, _drv_notice_, ("rtl8188eu_oid_rt_pro_set_channel_direct_call_hdl: Channel=%d\n", Channel));
+ if (Channel > 14)
+ return NDIS_STATUS_NOT_ACCEPTED;
+ Adapter->mppriv.channel = Channel;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetChannel(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u16 bandwidth;
+ u16 channel_offset;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *padapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("+rtl8188eu_oid_rt_set_bandwidth_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ bandwidth = *((u32 *)poid_par_priv->information_buf);/* 4 */
+ channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+ if (bandwidth != HT_CHANNEL_WIDTH_40)
+ bandwidth = HT_CHANNEL_WIDTH_20;
+ padapter->mppriv.bandwidth = (u8)bandwidth;
+ padapter->mppriv.prime_channel_offset = (u8)channel_offset;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetBandwidth(padapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("-rtl8188eu_oid_rt_set_bandwidth_hdl: bandwidth=%d channel_offset=%d\n",
+ bandwidth, channel_offset));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 antenna;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_set_antenna_bb_hdl\n"));
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ if (poid_par_priv->type_of_oid == SET_OID) {
+ antenna = *(u32 *)poid_par_priv->information_buf;
+
+ Adapter->mppriv.antenna_tx = (u16)((antenna & 0xFFFF0000) >> 16);
+ Adapter->mppriv.antenna_rx = (u16)(antenna & 0x0000FFFF);
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_set_antenna_bb_hdl: tx_ant=0x%04x rx_ant=0x%04x\n",
+ Adapter->mppriv.antenna_tx, Adapter->mppriv.antenna_rx));
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetAntenna(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+ } else {
+ antenna = (Adapter->mppriv.antenna_tx << 16)|Adapter->mppriv.antenna_rx;
+ *(u32 *)poid_par_priv->information_buf = antenna;
+ }
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_tx_power_control_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 tx_pwr_idx;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_, ("+rtl8188eu_oid_rt_pro_set_tx_power_control_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ tx_pwr_idx = *((u32 *)poid_par_priv->information_buf);
+ if (tx_pwr_idx > MAX_TX_PWR_INDEX_N_MODE)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ Adapter->mppriv.txpoweridx = (u8)tx_pwr_idx;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_set_tx_power_control_hdl: idx=0x%2x\n",
+ Adapter->mppriv.txpoweridx));
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetTxPower(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+
+/* */
+/* rtl8188eu_oid_rtl_seg_81_80_20 section start **************** */
+/* */
+int rtl8188eu_oid_rt_pro_query_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+
+ if (poid_par_priv->information_buf_len == sizeof(u32)) {
+ *(u32 *)poid_par_priv->information_buf = Adapter->mppriv.tx_pktcount;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ } else {
+ status = NDIS_STATUS_INVALID_LENGTH;
+ }
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_query_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+ RT_TRACE(_module_mp_, _drv_alert_, ("===> rtl8188eu_oid_rt_pro_query_rx_packet_received_hdl.\n"));
+ if (poid_par_priv->information_buf_len == sizeof(u32)) {
+ *(u32 *)poid_par_priv->information_buf = Adapter->mppriv.rx_pktcount;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ RT_TRACE(_module_mp_, _drv_alert_, ("recv_ok:%d\n", Adapter->mppriv.rx_pktcount));
+ } else {
+ status = NDIS_STATUS_INVALID_LENGTH;
+ }
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_query_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+ RT_TRACE(_module_mp_, _drv_alert_, ("===> rtl8188eu_oid_rt_pro_query_rx_packet_crc32_error_hdl.\n"));
+ if (poid_par_priv->information_buf_len == sizeof(u32)) {
+ *(u32 *)poid_par_priv->information_buf = Adapter->mppriv.rx_crcerrpktcount;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ RT_TRACE(_module_mp_, _drv_alert_, ("recv_err:%d\n", Adapter->mppriv.rx_crcerrpktcount));
+ } else {
+ status = NDIS_STATUS_INVALID_LENGTH;
+ }
+
+ return status;
+}
+/* */
+
+int rtl8188eu_oid_rt_pro_reset_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != SET_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+
+ RT_TRACE(_module_mp_, _drv_alert_, ("===> rtl8188eu_oid_rt_pro_reset_tx_packet_sent_hdl.\n"));
+ Adapter->mppriv.tx_pktcount = 0;
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_reset_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != SET_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+
+ if (poid_par_priv->information_buf_len == sizeof(u32)) {
+ Adapter->mppriv.rx_pktcount = 0;
+ Adapter->mppriv.rx_crcerrpktcount = 0;
+ } else {
+ status = NDIS_STATUS_INVALID_LENGTH;
+ }
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_reset_phy_rx_packet_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != SET_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ ResetPhyRxPktCount(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_phy_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_, ("+rtl8188eu_oid_rt_get_phy_rx_packet_received_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ *(u32 *)poid_par_priv->information_buf = GetPhyRxPktReceived(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("-rtl8188eu_oid_rt_get_phy_rx_packet_received_hdl: recv_ok=%d\n", *(u32 *)poid_par_priv->information_buf));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_phy_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_, ("+rtl8188eu_oid_rt_get_phy_rx_packet_crc32_error_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len != sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ *(u32 *)poid_par_priv->information_buf = GetPhyRxPktCRC32Error(Adapter);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("-rtl8188eu_oid_rt_get_phy_rx_packet_crc32_error_hdl: recv_err =%d\n",
+ *(u32 *)poid_par_priv->information_buf));
+
+ return status;
+}
+/* rtl8188eu_oid_rtl_seg_81_80_20 section end **************** */
+int rtl8188eu_oid_rt_pro_set_continuous_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 bStartTest;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_set_continuous_tx_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ bStartTest = *((u32 *)poid_par_priv->information_buf);
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetContinuousTx(Adapter, (u8)bStartTest);
+ if (bStartTest) {
+ struct mp_priv *pmp_priv = &Adapter->mppriv;
+ if (pmp_priv->tx.stop == 0) {
+ pmp_priv->tx.stop = 1;
+ DBG_88E("%s: pkt tx is running...\n", __func__);
+ rtw_msleep_os(5);
+ }
+ pmp_priv->tx.stop = 0;
+ pmp_priv->tx.count = 1;
+ SetPacketTx(Adapter);
+ }
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_single_carrier_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 bStartTest;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_alert_, ("+rtl8188eu_oid_rt_pro_set_single_carrier_tx_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ bStartTest = *((u32 *)poid_par_priv->information_buf);
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetSingleCarrierTx(Adapter, (u8)bStartTest);
+ if (bStartTest) {
+ struct mp_priv *pmp_priv = &Adapter->mppriv;
+ if (pmp_priv->tx.stop == 0) {
+ pmp_priv->tx.stop = 1;
+ DBG_88E("%s: pkt tx is running...\n", __func__);
+ rtw_msleep_os(5);
+ }
+ pmp_priv->tx.stop = 0;
+ pmp_priv->tx.count = 1;
+ SetPacketTx(Adapter);
+ }
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_carrier_suppression_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 bStartTest;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_set_carrier_suppression_tx_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ bStartTest = *((u32 *)poid_par_priv->information_buf);
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetCarrierSuppressionTx(Adapter, (u8)bStartTest);
+ if (bStartTest) {
+ struct mp_priv *pmp_priv = &Adapter->mppriv;
+ if (pmp_priv->tx.stop == 0) {
+ pmp_priv->tx.stop = 1;
+ DBG_88E("%s: pkt tx is running...\n", __func__);
+ rtw_msleep_os(5);
+ }
+ pmp_priv->tx.stop = 0;
+ pmp_priv->tx.count = 1;
+ SetPacketTx(Adapter);
+ }
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_single_tone_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u32 bStartTest;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_alert_, ("+rtl8188eu_oid_rt_pro_set_single_tone_tx_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ bStartTest = *((u32 *)poid_par_priv->information_buf);
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ SetSingleToneTx(Adapter, (u8)bStartTest);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_modulation_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_trigger_gpio_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+ int status = NDIS_STATUS_SUCCESS;
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ rtw_hal_set_hwreg(Adapter, HW_VAR_TRIGGER_GPIO_0, NULL);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* rtl8188eu_oid_rtl_seg_81_80_00 section end **************** */
+/* */
+int rtl8188eu_oid_rt_pro8711_join_bss_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct mp_rw_reg *RegRWStruct;
+ u32 offset, width;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("+rtl8188eu_oid_rt_pro_read_register_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ RegRWStruct = (struct mp_rw_reg *)poid_par_priv->information_buf;
+ offset = RegRWStruct->offset;
+ width = RegRWStruct->width;
+
+ if (offset > 0xFFF)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ switch (width) {
+ case 1:
+ RegRWStruct->value = rtw_read8(Adapter, offset);
+ break;
+ case 2:
+ RegRWStruct->value = rtw_read16(Adapter, offset);
+ break;
+ default:
+ width = 4;
+ RegRWStruct->value = rtw_read32(Adapter, offset);
+ break;
+ }
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_read_register_hdl: offset:0x%04X value:0x%X\n",
+ offset, RegRWStruct->value));
+
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ *poid_par_priv->bytes_rw = width;
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct mp_rw_reg *RegRWStruct;
+ u32 offset, width, value;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *padapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("+rtl8188eu_oid_rt_pro_write_register_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ RegRWStruct = (struct mp_rw_reg *)poid_par_priv->information_buf;
+ offset = RegRWStruct->offset;
+ width = RegRWStruct->width;
+ value = RegRWStruct->value;
+
+ if (offset > 0xFFF)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ switch (RegRWStruct->width) {
+ case 1:
+ if (value > 0xFF) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ break;
+ }
+ rtw_write8(padapter, offset, (u8)value);
+ break;
+ case 2:
+ if (value > 0xFFFF) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ break;
+ }
+ rtw_write16(padapter, offset, (u16)value);
+ break;
+ case 4:
+ rtw_write32(padapter, offset, value);
+ break;
+ default:
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ break;
+ }
+
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("-rtl8188eu_oid_rt_pro_write_register_hdl: offset=0x%08X width=%d value=0x%X\n",
+ offset, width, value));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_burst_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_burst_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_write_txcmd_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+/* */
+int rtl8188eu_oid_rt_pro_read16_eeprom_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+/* */
+int rtl8188eu_oid_rt_pro_write16_eeprom_hdl (struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro8711_wi_poll_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro8711_pkt_loss_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_rd_attrib_mem_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_wr_attrib_mem_hdl (struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_rf_intfs_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_cfg_debug_message_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_data_rate_ex_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ int status = NDIS_STATUS_SUCCESS;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+OID_RT_PRO_SET_DATA_RATE_EX\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ if (rtw_setdatarate_cmd(Adapter, poid_par_priv->information_buf) != _SUCCESS)
+ status = NDIS_STATUS_NOT_ACCEPTED;
+
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ u8 thermal = 0;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_get_thermal_meter_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ GetThermalMeter(Adapter, &thermal);
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ *(u32 *)poid_par_priv->information_buf = (u32)thermal;
+ *poid_par_priv->bytes_rw = sizeof(u32);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_read_tssi_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_power_tracking_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->information_buf_len < sizeof(u8))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ if (poid_par_priv->type_of_oid == SET_OID) {
+ u8 enable;
+
+ enable = *(u8 *)poid_par_priv->information_buf;
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("+rtl8188eu_oid_rt_pro_set_power_tracking_hdl: enable =%d\n", enable));
+
+ SetPowerTracking(Adapter, enable);
+ } else {
+ GetPowerTracking(Adapter, (u8 *)poid_par_priv->information_buf);
+ }
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_basic_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_qry_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_set_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_h2c_set_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+/* */
+int rtl8188eu_oid_rt_pro_h2c_get_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+/* rtl8188eu_oid_rtl_seg_87_12_00 section start **************** */
+int rtl8188eu_oid_rt_pro_encryption_ctrl_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_add_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return NDIS_STATUS_SUCCESS;
+}
+/* */
+int rtl8188eu_oid_rt_pro_read_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct efuse_access_struct *pefuse;
+ u8 *data;
+ u16 addr = 0, cnts = 0, max_available_size = 0;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct efuse_access_struct))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ pefuse = (struct efuse_access_struct *)poid_par_priv->information_buf;
+ addr = pefuse->start_addr;
+ cnts = pefuse->cnts;
+ data = pefuse->data;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("+rtl8188eu_oid_rt_pro_read_efuse_hd: buf_len=%d addr=%d cnts=%d\n",
+ poid_par_priv->information_buf_len, addr, cnts));
+
+ EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_available_size, false);
+
+ if ((addr + cnts) > max_available_size) {
+ RT_TRACE(_module_mp_, _drv_err_, ("!rtl8188eu_oid_rt_pro_read_efuse_hdl: parameter error!\n"));
+ return NDIS_STATUS_NOT_ACCEPTED;
+ }
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ if (rtw_efuse_access(Adapter, false, addr, cnts, data) == _FAIL) {
+ RT_TRACE(_module_mp_, _drv_err_, ("!rtl8188eu_oid_rt_pro_read_efuse_hdl: rtw_efuse_access FAIL!\n"));
+ status = NDIS_STATUS_FAILURE;
+ } else {
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ }
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_write_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct efuse_access_struct *pefuse;
+ u8 *data;
+ u16 addr = 0, cnts = 0, max_available_size = 0;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ pefuse = (struct efuse_access_struct *)poid_par_priv->information_buf;
+ addr = pefuse->start_addr;
+ cnts = pefuse->cnts;
+ data = pefuse->data;
+
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("+rtl8188eu_oid_rt_pro_write_efuse_hdl: buf_len=%d addr=0x%04x cnts=%d\n",
+ poid_par_priv->information_buf_len, addr, cnts));
+
+ EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_available_size, false);
+
+ if ((addr + cnts) > max_available_size) {
+ RT_TRACE(_module_mp_, _drv_err_, ("!rtl8188eu_oid_rt_pro_write_efuse_hdl: parameter error"));
+ return NDIS_STATUS_NOT_ACCEPTED;
+ }
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ if (rtw_efuse_access(Adapter, true, addr, cnts, data) == _FAIL)
+ status = NDIS_STATUS_FAILURE;
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_rw_efuse_pgpkt_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct pgpkt *ppgpkt;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ *poid_par_priv->bytes_rw = 0;
+
+ if (poid_par_priv->information_buf_len < sizeof(struct pgpkt *))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ ppgpkt = (struct pgpkt *)poid_par_priv->information_buf;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ if (poid_par_priv->type_of_oid == QUERY_OID) {
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_rw_efuse_pgpkt_hdl: Read offset=0x%x\n",\
+ ppgpkt->offset));
+
+ Efuse_PowerSwitch(Adapter, false, true);
+ if (Efuse_PgPacketRead(Adapter, ppgpkt->offset, ppgpkt->data, false) == true)
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ else
+ status = NDIS_STATUS_FAILURE;
+ Efuse_PowerSwitch(Adapter, false, false);
+ } else {
+ RT_TRACE(_module_mp_, _drv_notice_,
+ ("rtl8188eu_oid_rt_pro_rw_efuse_pgpkt_hdl: Write offset=0x%x word_en=0x%x\n",\
+ ppgpkt->offset, ppgpkt->word_en));
+
+ Efuse_PowerSwitch(Adapter, true, true);
+ if (Efuse_PgPacketWrite(Adapter, ppgpkt->offset, ppgpkt->word_en, ppgpkt->data, false) == true)
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ else
+ status = NDIS_STATUS_FAILURE;
+ Efuse_PowerSwitch(Adapter, true, false);
+ }
+
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("-rtl8188eu_oid_rt_pro_rw_efuse_pgpkt_hdl: status=0x%08X\n", status));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_efuse_current_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u16 size;
+ u8 ret;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+ ret = efuse_GetCurrentSize(Adapter, &size);
+ _irqlevel_changed_(&oldirql, RAISE);
+ if (ret == _SUCCESS) {
+ *(u32 *)poid_par_priv->information_buf = size;
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+ } else {
+ status = NDIS_STATUS_FAILURE;
+ }
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+ if (poid_par_priv->type_of_oid != QUERY_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(u32))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ *(u32 *)poid_par_priv->information_buf = efuse_GetMaxSize(Adapter);
+ *poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("-rtl8188eu_oid_rt_get_efuse_max_size_hdl: size=%d status=0x%08X\n",
+ *(int *)poid_par_priv->information_buf, status));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status;
+
+ RT_TRACE(_module_mp_, _drv_info_, ("+rtl8188eu_oid_rt_pro_efuse_hdl\n"));
+
+ if (poid_par_priv->type_of_oid == QUERY_OID)
+ status = rtl8188eu_oid_rt_pro_read_efuse_hdl(poid_par_priv);
+ else
+ status = rtl8188eu_oid_rt_pro_write_efuse_hdl(poid_par_priv);
+
+ RT_TRACE(_module_mp_, _drv_info_, ("-rtl8188eu_oid_rt_pro_efuse_hdl: status=0x%08X\n", status));
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_pro_efuse_map_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u8 *data;
+ int status = NDIS_STATUS_SUCCESS;
+ struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+ u16 maplen = 0;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_pro_efuse_map_hdl\n"));
+
+ EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&maplen, false);
+
+ *poid_par_priv->bytes_rw = 0;
+
+ if (poid_par_priv->information_buf_len < maplen)
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ data = (u8 *)poid_par_priv->information_buf;
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ if (poid_par_priv->type_of_oid == QUERY_OID) {
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("rtl8188eu_oid_rt_pro_efuse_map_hdl: READ\n"));
+
+ if (rtw_efuse_map_read(Adapter, 0, maplen, data) == _SUCCESS) {
+ *poid_par_priv->bytes_rw = maplen;
+ } else {
+ RT_TRACE(_module_mp_, _drv_err_,
+ ("rtl8188eu_oid_rt_pro_efuse_map_hdl: READ fail\n"));
+ status = NDIS_STATUS_FAILURE;
+ }
+ } else {
+ /* SET_OID */
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("rtl8188eu_oid_rt_pro_efuse_map_hdl: WRITE\n"));
+
+ if (rtw_efuse_map_write(Adapter, 0, maplen, data) == _SUCCESS) {
+ *poid_par_priv->bytes_rw = maplen;
+ } else {
+ RT_TRACE(_module_mp_, _drv_err_,
+ ("rtl8188eu_oid_rt_pro_efuse_map_hdl: WRITE fail\n"));
+ status = NDIS_STATUS_FAILURE;
+ }
+ }
+
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("-rtl8188eu_oid_rt_pro_efuse_map_hdl: status=0x%08X\n", status));
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+ return status;
+}
+
+int rtl8188eu_oid_rt_set_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+ u8 rx_pkt_type;
+ int status = NDIS_STATUS_SUCCESS;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+rtl8188eu_oid_rt_set_rx_packet_type_hdl\n"));
+
+ if (poid_par_priv->type_of_oid != SET_OID)
+ return NDIS_STATUS_NOT_ACCEPTED;
+
+ if (poid_par_priv->information_buf_len < sizeof(u8))
+ return NDIS_STATUS_INVALID_LENGTH;
+
+ rx_pkt_type = *((u8 *)poid_par_priv->information_buf);/* 4 */
+
+ RT_TRACE(_module_mp_, _drv_info_, ("rx_pkt_type: %x\n", rx_pkt_type));
+
+ return status;
+}
+
+int rtl8188eu_oid_rt_pro_set_tx_agc_offset_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_oid_rt_pro_set_pkt_test_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
+
+int rtl8188eu_mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+ struct mp_xmit_parm *pparm;
+ struct adapter *padapter;
+ struct mp_priv *pmp_priv;
+ struct pkt_attrib *pattrib;
+
+ RT_TRACE(_module_mp_, _drv_notice_, ("+%s\n", __func__));
+
+ pparm = (struct mp_xmit_parm *)poid_par_priv->information_buf;
+ padapter = (struct adapter *)poid_par_priv->adapter_context;
+ pmp_priv = &padapter->mppriv;
+
+ if (poid_par_priv->type_of_oid == QUERY_OID) {
+ pparm->enable = !pmp_priv->tx.stop;
+ pparm->count = pmp_priv->tx.sended;
+ } else {
+ if (pparm->enable == 0) {
+ pmp_priv->tx.stop = 1;
+ } else if (pmp_priv->tx.stop == 1) {
+ pmp_priv->tx.stop = 0;
+ pmp_priv->tx.count = pparm->count;
+ pmp_priv->tx.payload = pparm->payload_type;
+ pattrib = &pmp_priv->tx.attrib;
+ pattrib->pktlen = pparm->length;
+ memcpy(pattrib->dst, pparm->da, ETH_ALEN);
+ SetPacketTx(padapter);
+ } else {
+ return NDIS_STATUS_FAILURE;
+ }
+ }
+
+ return NDIS_STATUS_SUCCESS;
+}
+
+/* */
+int rtl8188eu_oid_rt_set_power_down_hdl(struct oid_par_priv *poid_par_priv)
+{
+ int status = NDIS_STATUS_SUCCESS;
+
+ if (poid_par_priv->type_of_oid != SET_OID) {
+ status = NDIS_STATUS_NOT_ACCEPTED;
+ return status;
+ }
+
+ RT_TRACE(_module_mp_, _drv_info_,
+ ("\n ===> Setrtl8188eu_oid_rt_set_power_down_hdl.\n"));
+
+ _irqlevel_changed_(&oldirql, LOWER);
+
+ /* CALL the power_down function */
+ _irqlevel_changed_(&oldirql, RAISE);
+
+ return status;
+}
+/* */
+int rtl8188eu_oid_rt_get_power_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+ return 0;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_p2p.c b/drivers/staging/r8188eu/core/rtw_p2p.c
new file mode 100644
index 000000000000..2e21496fe71b
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_p2p.c
@@ -0,0 +1,2068 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_P2P_C_
+
+#include <drv_types.h>
+#include <rtw_p2p.h>
+#include <wifi.h>
+
+#ifdef CONFIG_88EU_P2P
+
+static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8 *ch_list, u8 ch_cnt)
+{
+ int found = 0, i = 0;
+
+ for (i = 0; i < ch_cnt; i++) {
+ if (ch_list[i] == desired_ch) {
+ found = 1;
+ break;
+ }
+ }
+ return found;
+}
+
+static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+ struct list_head *phead, *plist;
+ u32 len = 0;
+ u16 attr_len = 0;
+ u8 tmplen, *pdata_attr, *pstart, *pcur;
+ struct sta_info *psta = NULL;
+ struct adapter *padapter = pwdinfo->padapter;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ DBG_88E("%s\n", __func__);
+
+ pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
+
+ pstart = pdata_attr;
+ pcur = pdata_attr;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* look up sta asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+
+ plist = plist->next;
+
+ if (psta->is_p2p_device) {
+ tmplen = 0;
+
+ pcur++;
+
+ /* P2P device address */
+ memcpy(pcur, psta->dev_addr, ETH_ALEN);
+ pcur += ETH_ALEN;
+
+ /* P2P interface address */
+ memcpy(pcur, psta->hwaddr, ETH_ALEN);
+ pcur += ETH_ALEN;
+
+ *pcur = psta->dev_cap;
+ pcur++;
+
+ /* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
+ RTW_PUT_BE16(pcur, psta->config_methods);
+ pcur += 2;
+
+ memcpy(pcur, psta->primary_dev_type, 8);
+ pcur += 8;
+
+ *pcur = psta->num_of_secdev_type;
+ pcur++;
+
+ memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
+ pcur += psta->num_of_secdev_type*8;
+
+ if (psta->dev_name_len > 0) {
+ /* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+ RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
+ pcur += 2;
+
+ /* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
+ RTW_PUT_BE16(pcur, psta->dev_name_len);
+ pcur += 2;
+
+ memcpy(pcur, psta->dev_name, psta->dev_name_len);
+ pcur += psta->dev_name_len;
+ }
+
+ tmplen = (u8)(pcur-pstart);
+
+ *pstart = (tmplen-1);
+
+ attr_len += tmplen;
+
+ /* pstart += tmplen; */
+ pstart = pcur;
+ }
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+ if (attr_len > 0)
+ len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
+
+ kfree(pdata_attr);
+ return len;
+}
+
+static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct adapter *padapter = pwdinfo->padapter;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_GO_DISC_REQUEST;
+ u8 dialogToken = 0;
+
+ DBG_88E("[%s]\n", __func__);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ /* Build P2P action frame header */
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ /* there is no IE in this P2P action frame */
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct adapter *padapter = pwdinfo->padapter;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_DEVDISC_RESP;
+ u8 p2pie[8] = { 0x00 };
+ u32 p2pielen = 0;
+
+ DBG_88E("[%s]\n", __func__);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ /* Build P2P public action frame header */
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ /* Build P2P IE */
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* P2P_ATTR_STATUS */
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8 *raddr, u8 *frame_body, u16 config_method)
+{
+ struct adapter *padapter = pwdinfo->padapter;
+ unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+ u8 action = P2P_PUB_ACTION_ACTION;
+ u8 dialogToken = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_PROVISION_DISC_RESP;
+ u8 wpsie[100] = { 0x00 };
+ u8 wpsielen = 0;
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ wpsielen = 0;
+ /* WPS OUI */
+ RTW_PUT_BE32(wpsie, WPSOUI);
+ wpsielen += 4;
+
+ /* Config Method */
+ /* Type: */
+ RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
+ wpsielen += 2;
+
+ /* Length: */
+ RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
+ wpsielen += 2;
+
+ /* Value: */
+ RTW_PUT_BE16(wpsie + wpsielen, config_method);
+ wpsielen += 2;
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+
+ return;
+}
+
+static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{
+ struct xmit_frame *pmgntframe;
+ struct pkt_attrib *pattrib;
+ unsigned char *pframe;
+ struct rtw_ieee80211_hdr *pwlanhdr;
+ __le16 *fctrl;
+ struct adapter *padapter = pwdinfo->padapter;
+ struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
+ __be32 p2poui = cpu_to_be32(P2POUI);
+ u8 oui_subtype = P2P_PRESENCE_RESPONSE;
+ u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+ u8 noa_attr_content[32] = { 0x00 };
+ u32 p2pielen = 0;
+
+ DBG_88E("[%s]\n", __func__);
+
+ pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+ if (pmgntframe == NULL)
+ return;
+
+ /* update attribute */
+ pattrib = &pmgntframe->attrib;
+ update_mgntframe_attrib(padapter, pattrib);
+
+ memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+ pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+ pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+ fctrl = &(pwlanhdr->frame_ctl);
+ *(fctrl) = 0;
+
+ memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+ SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+ pmlmeext->mgnt_seq++;
+ SetFrameSubType(pframe, WIFI_ACTION);
+
+ pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+ pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ /* Build P2P action frame header */
+ pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+ pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+ /* Add P2P IE header */
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Add Status attribute in P2P IE */
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+ /* Add NoA attribute in P2P IE */
+ noa_attr_content[0] = 0x1;/* index */
+ noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
+
+ /* todo: Notice of Absence Descriptor(s) */
+
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
+
+ pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &(pattrib->pktlen));
+
+ pattrib->last_txcmdsz = pattrib->pktlen;
+
+ dump_mgntframe(padapter, pmgntframe);
+}
+
+u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+ u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+ u16 capability = 0;
+ u32 len = 0, p2pielen = 0;
+ __le16 le_tmp;
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. P2P Device ID */
+ /* 3. Notice of Absence (NOA) */
+
+ /* P2P Capability ATTR */
+ /* Type: */
+ /* Length: */
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ /* Be able to participate in additional P2P Groups and */
+ /* support the P2P Invitation Procedure */
+ /* Group Capability Bitmap, 1 byte */
+ capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
+ capability |= ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+ capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
+
+ le_tmp = cpu_to_le16(capability);
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8 *)&le_tmp);
+
+ /* P2P Device ID ATTR */
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
+
+ /* Notice of Absence ATTR */
+ /* Type: */
+ /* Length: */
+ /* Value: */
+
+ pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+ return len;
+}
+
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+ u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+ u32 len = 0, p2pielen = 0;
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20100907 */
+ /* According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. Extended Listen Timing */
+ /* 3. Notice of Absence (NOA) (Only GO needs this) */
+ /* 4. Device Info */
+ /* 5. Group Info (Only GO need this) */
+
+ /* P2P Capability ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
+ RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ p2pie[p2pielen] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+ p2pie[p2pielen] |= P2P_GRPCAP_GROUP_FORMATION;
+
+ p2pielen++;
+ } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+ else
+ p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+ }
+
+ /* Extended Listen Timing ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
+ RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Availability Period */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
+ RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+ p2pielen += 2;
+
+ /* Availability Interval */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
+ RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+ p2pielen += 2;
+
+ /* Notice of Absence ATTR */
+ /* Type: */
+ /* Length: */
+ /* Value: */
+
+ /* Device Info ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
+ RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
+ RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
+ RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
+ RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ /* Group Info ATTR */
+ /* Type: */
+ /* Length: */
+ /* Value: */
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+ p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
+
+ pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+ return len;
+}
+
+u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
+{
+ u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+ u32 len = 0, p2pielen = 0;
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* Commented by Albert 20110301 */
+ /* According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
+ /* 1. P2P Capability */
+ /* 2. Device Info */
+ /* 3. Group ID (When joining an operating P2P Group) */
+
+ /* P2P Capability ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
+ RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+ p2pielen += 2;
+
+ /* Value: */
+ /* Device Capability Bitmap, 1 byte */
+ p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+ /* Group Capability Bitmap, 1 byte */
+ if (pwdinfo->persistent_supported)
+ p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+ else
+ p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+ /* Device Info ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+ /* Length: */
+ /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
+ /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
+ RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ /* P2P Device Address */
+ memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ /* Config Method */
+ /* This field should be big endian. Noted by P2P specification. */
+ if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) {
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
+ } else {
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
+ }
+
+ p2pielen += 2;
+
+ /* Primary Device Type */
+ /* Category ID */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+ p2pielen += 2;
+
+ /* OUI */
+ /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
+ RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+ p2pielen += 4;
+
+ /* Sub Category ID */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+ p2pielen += 2;
+
+ /* Number of Secondary Device Types */
+ p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */
+
+ /* Device Name */
+ /* Type: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
+ RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+ p2pielen += 2;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
+ RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+ p2pielen += pwdinfo->device_name_len;
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+ /* Added by Albert 2011/05/19 */
+ /* In this case, the pdev_raddr is the device address of the group owner. */
+
+ /* P2P Group ID ATTR */
+ /* Type: */
+ p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+ /* Length: */
+ /* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
+ RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
+ p2pielen += 2;
+
+ /* Value: */
+ memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
+ p2pielen += ETH_ALEN;
+
+ memcpy(p2pie + p2pielen, pssid, ussidlen);
+ p2pielen += ussidlen;
+ }
+
+ pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+ return len;
+}
+
+u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
+{
+ u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+ u32 len = 0, p2pielen = 0;
+
+ /* P2P OUI */
+ p2pielen = 0;
+ p2pie[p2pielen++] = 0x50;
+ p2pie[p2pielen++] = 0x6F;
+ p2pie[p2pielen++] = 0x9A;
+ p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */
+
+ /* According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
+ /* 1. Status */
+ /* 2. Extended Listen Timing (optional) */
+
+ /* Status ATTR */
+ p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
+
+ /* Extended Listen Timing ATTR */
+ /* Type: */
+ /* Length: */
+ /* Value: */
+
+ pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+
+ return len;
+}
+
+u32 build_deauth_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+ u32 len = 0;
+
+ return len;
+}
+
+u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ u8 *p;
+ u32 ret = false;
+ u8 *p2pie;
+ u32 p2pielen = 0;
+ int ssid_len = 0, rate_cnt = 0;
+
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
+ len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+ if (rate_cnt <= 4) {
+ int i, g_rate = 0;
+
+ for (i = 0; i < rate_cnt; i++) {
+ if (((*(p + 2 + i) & 0xff) != 0x02) &&
+ ((*(p + 2 + i) & 0xff) != 0x04) &&
+ ((*(p + 2 + i) & 0xff) != 0x0B) &&
+ ((*(p + 2 + i) & 0xff) != 0x16))
+ g_rate = 1;
+ }
+
+ if (g_rate == 0) {
+ /* There is no OFDM rate included in SupportedRates IE of this probe request frame */
+ /* The driver should response this probe request. */
+ return ret;
+ }
+ } else {
+ /* rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
+ /* We should proceed the following check for this probe request. */
+ }
+
+ /* Added comments by Albert 20100906 */
+ /* There are several items we should check here. */
+ /* 1. This probe request frame must contain the P2P IE. (Done) */
+ /* 2. This probe request frame must contain the wildcard SSID. (Done) */
+ /* 3. Wildcard BSSID. (Todo) */
+ /* 4. Destination Address. (Done in mgt_dispatcher function) */
+ /* 5. Requested Device Type in WSC IE. (Todo) */
+ /* 6. Device ID attribute in P2P IE. (Todo) */
+
+ p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
+ len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+ ssid_len &= 0xff; /* Just last 1 byte is valid for ssid len of the probe request */
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen);
+ if (p2pie) {
+ if ((p != NULL) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid , 7)) {
+ /* todo: */
+ /* Check Requested Device Type attributes in WSC IE. */
+ /* Check Device ID attribute in P2P IE */
+
+ ret = true;
+ } else if ((p != NULL) && (ssid_len == 0)) {
+ ret = true;
+ }
+ } else {
+ /* non -p2p device */
+ }
+ }
+
+ return ret;
+}
+
+u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
+{
+ u8 status_code = P2P_STATUS_SUCCESS;
+ u8 *pbuf, *pattr_content = NULL;
+ u32 attr_contentlen = 0;
+ u16 cap_attr = 0;
+ unsigned short frame_type, ie_offset = 0;
+ u8 *ies;
+ u32 ies_len;
+ u8 *p2p_ie;
+ u32 p2p_ielen = 0;
+ __be16 be_tmp;
+ __le16 le_tmp;
+
+ if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+ return P2P_STATUS_FAIL_REQUEST_UNABLE;
+
+ frame_type = GetFrameSubType(pframe);
+ if (frame_type == WIFI_ASSOCREQ)
+ ie_offset = _ASOCREQ_IE_OFFSET_;
+ else /* WIFI_REASSOCREQ */
+ ie_offset = _REASOCREQ_IE_OFFSET_;
+
+ ies = pframe + WLAN_HDR_A3_LEN + ie_offset;
+ ies_len = len - WLAN_HDR_A3_LEN - ie_offset;
+
+ p2p_ie = rtw_get_p2p_ie(ies , ies_len , NULL, &p2p_ielen);
+
+ if (!p2p_ie) {
+ DBG_88E("[%s] P2P IE not Found!!\n", __func__);
+ status_code = P2P_STATUS_FAIL_INVALID_PARAM;
+ } else {
+ DBG_88E("[%s] P2P IE Found!!\n", __func__);
+ }
+
+ while (p2p_ie) {
+ /* Check P2P Capability ATTR */
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
+ DBG_88E("[%s] Got P2P Capability Attr!!\n", __func__);
+ cap_attr = le16_to_cpu(le_tmp);
+ psta->dev_cap = cap_attr&0xff;
+ }
+
+ /* Check Extended Listen Timing ATTR */
+
+ /* Check P2P Device Info ATTR */
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint *)&attr_contentlen)) {
+ DBG_88E("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
+ pattr_content = rtw_zmalloc(attr_contentlen);
+ pbuf = pattr_content;
+ if (pattr_content) {
+ u8 num_of_secdev_type;
+ u16 dev_name_len;
+
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint *)&attr_contentlen);
+
+ memcpy(psta->dev_addr, pattr_content, ETH_ALEN);/* P2P Device Address */
+
+ pattr_content += ETH_ALEN;
+
+ memcpy(&be_tmp, pattr_content, 2);/* Config Methods */
+ psta->config_methods = be16_to_cpu(be_tmp);
+
+ pattr_content += 2;
+
+ memcpy(psta->primary_dev_type, pattr_content, 8);
+
+ pattr_content += 8;
+
+ num_of_secdev_type = *pattr_content;
+ pattr_content += 1;
+
+ if (num_of_secdev_type == 0) {
+ psta->num_of_secdev_type = 0;
+ } else {
+ u32 len;
+
+ psta->num_of_secdev_type = num_of_secdev_type;
+
+ len = (sizeof(psta->secdev_types_list) < (num_of_secdev_type*8)) ?
+ (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
+
+ memcpy(psta->secdev_types_list, pattr_content, len);
+
+ pattr_content += (num_of_secdev_type*8);
+ }
+
+ psta->dev_name_len = 0;
+ if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(__be16 *)pattr_content)) {
+ dev_name_len = be16_to_cpu(*(__be16 *)(pattr_content+2));
+
+ psta->dev_name_len = (sizeof(psta->dev_name) < dev_name_len) ? sizeof(psta->dev_name) : dev_name_len;
+
+ memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
+ }
+ kfree(pbuf);
+ }
+ }
+
+ /* Get the next P2P IE */
+ p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+ }
+
+ return status_code;
+}
+
+u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ u8 *frame_body;
+ u8 status, dialogToken;
+ struct sta_info *psta = NULL;
+ struct adapter *padapter = pwdinfo->padapter;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 *p2p_ie;
+ u32 p2p_ielen = 0;
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ dialogToken = frame_body[7];
+ status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+
+ p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+ if (p2p_ie) {
+ u8 groupid[38] = { 0x00 };
+ u8 dev_addr[ETH_ALEN] = { 0x00 };
+ u32 attr_contentlen = 0;
+
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+ if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
+ !memcmp(pwdinfo->p2p_group_ssid, groupid+ETH_ALEN, pwdinfo->p2p_group_ssid_len)) {
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen)) {
+ struct list_head *phead, *plist;
+
+ spin_lock_bh(&pstapriv->asoc_list_lock);
+ phead = &pstapriv->asoc_list;
+ plist = phead->next;
+
+ /* look up sta asoc_queue */
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, asoc_list);
+
+ plist = plist->next;
+
+ if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
+ !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) {
+ /* issue GO Discoverability Request */
+ issue_group_disc_req(pwdinfo, psta->hwaddr);
+ status = P2P_STATUS_SUCCESS;
+ break;
+ } else {
+ status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ }
+ }
+ spin_unlock_bh(&pstapriv->asoc_list_lock);
+ } else {
+ status = P2P_STATUS_FAIL_INVALID_PARAM;
+ }
+ } else {
+ status = P2P_STATUS_FAIL_INVALID_PARAM;
+ }
+ }
+ }
+
+ /* issue Device Discoverability Response */
+ issue_p2p_devdisc_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+
+ return (status == P2P_STATUS_SUCCESS) ? true : false;
+}
+
+u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ return true;
+}
+
+u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ u8 *frame_body;
+ u8 *wpsie;
+ uint wps_ielen = 0, attr_contentlen = 0;
+ u16 uconfig_method = 0;
+ __be16 be_tmp;
+
+ frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ wpsie = rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+ if (wpsie) {
+ if (rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, (u8 *)&be_tmp, &attr_contentlen)) {
+ uconfig_method = be16_to_cpu(be_tmp);
+ switch (uconfig_method) {
+ case WPS_CM_DISPLYA:
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+ break;
+ case WPS_CM_LABEL:
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3);
+ break;
+ case WPS_CM_PUSH_BUTTON:
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+ break;
+ case WPS_CM_KEYPAD:
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+ break;
+ }
+ issue_p2p_provision_resp(pwdinfo, GetAddr2Ptr(pframe), frame_body, uconfig_method);
+ }
+ }
+ DBG_88E("[%s] config method = %s\n", __func__, pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req);
+ return true;
+}
+
+u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe)
+{
+ return true;
+}
+
+static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
+{
+ u8 i = 0, j = 0;
+ u8 temp = 0;
+ u8 ch_no = 0;
+ ch_content += 3;
+ ch_cnt -= 3;
+
+ while (ch_cnt > 0) {
+ ch_content += 1;
+ ch_cnt -= 1;
+ temp = *ch_content;
+ for (i = 0 ; i < temp ; i++, j++)
+ peer_ch_list[j] = *(ch_content + 1 + i);
+ ch_content += (temp + 1);
+ ch_cnt -= (temp + 1);
+ ch_no += temp ;
+ }
+
+ return ch_no;
+}
+
+static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
+{
+ int i = 0, j = 0, temp = 0;
+ u8 ch_no = 0;
+
+ for (i = 0; i < peer_ch_num; i++) {
+ for (j = temp; j < pmlmeext->max_chan_nums; j++) {
+ if (*(peer_ch_list + i) == pmlmeext->channel_set[j].ChannelNum) {
+ ch_list_inclusioned[ch_no++] = *(peer_ch_list + i);
+ temp = j;
+ break;
+ }
+ }
+ }
+
+ return ch_no;
+}
+
+u8 process_p2p_group_negotation_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ struct adapter *padapter = pwdinfo->padapter;
+ u8 result = P2P_STATUS_SUCCESS;
+ u32 p2p_ielen = 0, wps_ielen = 0;
+ u8 *ies;
+ u32 ies_len;
+ u8 *p2p_ie;
+ u8 *wpsie;
+ u16 wps_devicepassword_id = 0x0000;
+ uint wps_devicepassword_id_len = 0;
+ __be16 be_tmp;
+
+ wpsie = rtw_get_wps_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+ if (wpsie) {
+ /* Commented by Kurt 20120113 */
+ /* If some device wants to do p2p handshake without sending prov_disc_req */
+ /* We have to get peer_req_cm from here. */
+ if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
+ rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
+ wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+ if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+ else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+ else
+ memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+ }
+ } else {
+ DBG_88E("[%s] WPS IE not Found!!\n", __func__);
+ result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ return result;
+ }
+
+ if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) {
+ result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
+ return result;
+ }
+
+ ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+ ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+ p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+ if (!p2p_ie) {
+ DBG_88E("[%s] P2P IE not Found!!\n", __func__);
+ result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ }
+
+ while (p2p_ie) {
+ u8 attr_content = 0x00;
+ u32 attr_contentlen = 0;
+ u8 ch_content[50] = { 0x00 };
+ uint ch_cnt = 0;
+ u8 peer_ch_list[50] = { 0x00 };
+ u8 peer_ch_num = 0;
+ u8 ch_list_inclusioned[50] = { 0x00 };
+ u8 ch_num_inclusioned = 0;
+
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
+ DBG_88E("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
+ pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */
+
+ if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
+ /* Try to match the tie breaker value */
+ if (pwdinfo->intent == P2P_MAX_INTENT) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+ } else {
+ if (attr_content & 0x01)
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ else
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ }
+ } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ }
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* Store the group id information. */
+ memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+ memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+ }
+ }
+
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+ if (attr_contentlen != ETH_ALEN)
+ memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+ }
+
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt)) {
+ peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
+ ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+ if (ch_num_inclusioned == 0) {
+ DBG_88E("[%s] No common channel in channel list!\n", __func__);
+ result = P2P_STATUS_FAIL_NO_COMMON_CH;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ break;
+ }
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
+ ch_list_inclusioned, ch_num_inclusioned)) {
+ u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+ attr_contentlen = 0;
+
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+ peer_operating_ch = operatingch_info[4];
+
+ if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+ ch_list_inclusioned, ch_num_inclusioned)) {
+ /**
+ * Change our operating channel as peer's for compatibility.
+ */
+ pwdinfo->operating_channel = peer_operating_ch;
+ DBG_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
+ } else {
+ /* Take first channel of ch_list_inclusioned as operating channel */
+ pwdinfo->operating_channel = ch_list_inclusioned[0];
+ DBG_88E("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
+ }
+ }
+ }
+ }
+
+ /* Get the next P2P IE */
+ p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+ }
+ return result;
+}
+
+u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ struct adapter *padapter = pwdinfo->padapter;
+ u8 result = P2P_STATUS_SUCCESS;
+ u32 p2p_ielen, wps_ielen;
+ u8 *ies;
+ u32 ies_len;
+ u8 *p2p_ie;
+
+ ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+ ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+ /* Be able to know which one is the P2P GO and which one is P2P client. */
+
+ if (rtw_get_wps_ie(ies, ies_len, NULL, &wps_ielen)) {
+ } else {
+ DBG_88E("[%s] WPS IE not Found!!\n", __func__);
+ result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ }
+
+ p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+ if (!p2p_ie) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+ } else {
+ u8 attr_content = 0x00;
+ u32 attr_contentlen = 0;
+ u8 operatingch_info[5] = { 0x00 };
+ u8 groupid[38];
+ u8 peer_ch_list[50] = { 0x00 };
+ u8 peer_ch_num = 0;
+ u8 ch_list_inclusioned[50] = { 0x00 };
+ u8 ch_num_inclusioned = 0;
+
+ while (p2p_ie) { /* Found the P2P IE. */
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+ if (attr_contentlen == 1) {
+ DBG_88E("[%s] Status = %d\n", __func__, attr_content);
+ if (attr_content == P2P_STATUS_SUCCESS) {
+ /* Do nothing. */
+ } else {
+ if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
+ } else {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ }
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ result = attr_content;
+ break;
+ }
+ }
+
+ /* Try to get the peer's interface address */
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+ if (attr_contentlen != ETH_ALEN)
+ memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+ }
+
+ /* Try to get the peer's intent and tie breaker value. */
+ attr_content = 0x00;
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
+ DBG_88E("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
+ pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */
+
+ if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
+ /* Try to match the tie breaker value */
+ if (pwdinfo->intent == P2P_MAX_INTENT) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ } else {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ if (attr_content & 0x01)
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ else
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ }
+ } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ } else {
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ }
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ /* Store the group id information. */
+ memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+ memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+ }
+ }
+
+ /* Try to get the operation channel information */
+
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
+ DBG_88E("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
+ pwdinfo->peer_operating_ch = operatingch_info[4];
+ }
+
+ /* Try to get the channel list information */
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) {
+ DBG_88E("[%s] channel list attribute found, len = %d\n", __func__, pwdinfo->channel_list_attr_len);
+
+ peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
+ ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+ if (ch_num_inclusioned == 0) {
+ DBG_88E("[%s] No common channel in channel list!\n", __func__);
+ result = P2P_STATUS_FAIL_NO_COMMON_CH;
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ break;
+ }
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+ if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
+ ch_list_inclusioned, ch_num_inclusioned)) {
+ u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+ attr_contentlen = 0;
+
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+ peer_operating_ch = operatingch_info[4];
+
+ if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+ ch_list_inclusioned, ch_num_inclusioned)) {
+ /**
+ * Change our operating channel as peer's for compatibility.
+ */
+ pwdinfo->operating_channel = peer_operating_ch;
+ DBG_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
+ } else {
+ /* Take first channel of ch_list_inclusioned as operating channel */
+ pwdinfo->operating_channel = ch_list_inclusioned[0];
+ DBG_88E("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
+ }
+ }
+ }
+ } else {
+ DBG_88E("[%s] channel list attribute not found!\n", __func__);
+ }
+
+ /* Try to get the group id information if peer is GO */
+ attr_contentlen = 0;
+ memset(groupid, 0x00, 38);
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+ memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+ memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
+ }
+
+ /* Get the next P2P IE */
+ p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+ }
+ }
+ return result;
+}
+
+u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ u8 *ies;
+ u32 ies_len;
+ u8 *p2p_ie;
+ u32 p2p_ielen = 0;
+ u8 result = P2P_STATUS_SUCCESS;
+ ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+ ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+ p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+ while (p2p_ie) { /* Found the P2P IE. */
+ u8 attr_content = 0x00, operatingch_info[5] = { 0x00 };
+ u8 groupid[38] = { 0x00 };
+ u32 attr_contentlen = 0;
+
+ pwdinfo->negotiation_dialog_token = 1;
+ rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+ if (attr_contentlen == 1) {
+ DBG_88E("[%s] Status = %d\n", __func__, attr_content);
+ result = attr_content;
+
+ if (attr_content == P2P_STATUS_SUCCESS) {
+ u8 bcancelled = 0;
+
+ _cancel_timer(&pwdinfo->restore_p2p_state_timer, &bcancelled);
+
+ /* Commented by Albert 20100911 */
+ /* Todo: Need to handle the case which both Intents are the same. */
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1)) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ } else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1)) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ } else {
+ /* Have to compare the Tie Breaker */
+ if (pwdinfo->peer_intent & 0x01)
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ else
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ }
+ } else {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+ break;
+ }
+ }
+
+ /* Try to get the group id information */
+ attr_contentlen = 0;
+ memset(groupid, 0x00, 38);
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+ DBG_88E("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
+ memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+ memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
+ }
+
+ attr_contentlen = 0;
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
+ DBG_88E("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
+ pwdinfo->peer_operating_ch = operatingch_info[4];
+ }
+
+ /* Get the next P2P IE */
+ p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+ }
+ return result;
+}
+
+u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+ u8 *frame_body;
+ u8 dialogToken = 0;
+ u8 status = P2P_STATUS_SUCCESS;
+
+ frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+ dialogToken = frame_body[6];
+
+ /* todo: check NoA attribute */
+
+ issue_p2p_presence_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+
+ return true;
+}
+
+static void find_phase_handler(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ndis_802_11_ssid ssid;
+
+ memset((unsigned char *)&ssid, 0, sizeof(struct ndis_802_11_ssid));
+ memcpy(ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
+ ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
+
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
+
+ spin_lock_bh(&pmlmepriv->lock);
+ spin_unlock_bh(&pmlmepriv->lock);
+
+}
+
+void p2p_concurrent_handler(struct adapter *padapter);
+
+static void restore_p2p_state_handler(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+ if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+ /* In the P2P client mode, the driver should not switch back to its listen channel */
+ /* because this P2P client should stay at the operating channel of P2P GO. */
+ set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ }
+
+}
+
+static void pre_tx_invitereq_handler(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+ u8 val8 = 1;
+
+ set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+ issue_probereq_p2p(padapter, NULL);
+ _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+static void pre_tx_provdisc_handler(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+ u8 val8 = 1;
+
+ set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+ issue_probereq_p2p(padapter, NULL);
+ _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+static void pre_tx_negoreq_handler(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+ u8 val8 = 1;
+
+ set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+ issue_probereq_p2p(padapter, NULL);
+ _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+void p2p_protocol_wk_hdl(struct adapter *padapter, int intCmdType)
+{
+
+ switch (intCmdType) {
+ case P2P_FIND_PHASE_WK:
+ find_phase_handler(padapter);
+ break;
+ case P2P_RESTORE_STATE_WK:
+ restore_p2p_state_handler(padapter);
+ break;
+ case P2P_PRE_TX_PROVDISC_PROCESS_WK:
+ pre_tx_provdisc_handler(padapter);
+ break;
+ case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
+ pre_tx_invitereq_handler(padapter);
+ break;
+ case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
+ pre_tx_negoreq_handler(padapter);
+ break;
+ }
+
+}
+
+void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
+{
+ u8 *ies;
+ u32 ies_len;
+ u8 *p2p_ie;
+ u32 p2p_ielen = 0;
+ u8 noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/* NoA length should be n*(13) + 2 */
+ u32 attr_contentlen = 0;
+
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ u8 find_p2p = false, find_p2p_ps = false;
+ u8 noa_offset, noa_num, noa_index;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+ if (IELength <= _BEACON_IE_OFFSET_)
+ return;
+
+ ies = IEs + _BEACON_IE_OFFSET_;
+ ies_len = IELength - _BEACON_IE_OFFSET_;
+
+ p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+ while (p2p_ie) {
+ find_p2p = true;
+ /* Get Notice of Absence IE. */
+ if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen)) {
+ find_p2p_ps = true;
+ noa_index = noa_attr[0];
+
+ if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
+ (noa_index != pwdinfo->noa_index)) { /* if index change, driver should reconfigure related setting. */
+ pwdinfo->noa_index = noa_index;
+ pwdinfo->opp_ps = noa_attr[1] >> 7;
+ pwdinfo->ctwindow = noa_attr[1] & 0x7F;
+
+ noa_offset = 2;
+ noa_num = 0;
+ /* NoA length should be n*(13) + 2 */
+ if (attr_contentlen > 2) {
+ while (noa_offset < attr_contentlen) {
+ /* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
+ pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
+ noa_offset += 1;
+
+ memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
+ noa_offset += 4;
+
+ memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
+ noa_offset += 4;
+
+ memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
+ noa_offset += 4;
+
+ noa_num++;
+ }
+ }
+ pwdinfo->noa_num = noa_num;
+
+ if (pwdinfo->opp_ps == 1) {
+ pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+ /* driver should wait LPS for entering CTWindow */
+ if (padapter->pwrctrlpriv.bFwCurrentInPSMode)
+ p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+ } else if (pwdinfo->noa_num > 0) {
+ pwdinfo->p2p_ps_mode = P2P_PS_NOA;
+ p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+ } else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+ p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+ }
+ }
+
+ break; /* find target, just break. */
+ }
+
+ /* Get the next P2P IE */
+ p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+ }
+
+ if (find_p2p) {
+ if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && !find_p2p_ps)
+ p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+ }
+
+}
+
+void p2p_ps_wk_hdl(struct adapter *padapter, u8 p2p_ps_state)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ /* Pre action for p2p state */
+ switch (p2p_ps_state) {
+ case P2P_PS_DISABLE:
+ pwdinfo->p2p_ps_state = p2p_ps_state;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+
+ pwdinfo->noa_index = 0;
+ pwdinfo->ctwindow = 0;
+ pwdinfo->opp_ps = 0;
+ pwdinfo->noa_num = 0;
+ pwdinfo->p2p_ps_mode = P2P_PS_NONE;
+ if (padapter->pwrctrlpriv.bFwCurrentInPSMode) {
+ if (pwrpriv->smart_ps == 0) {
+ pwrpriv->smart_ps = 2;
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
+ }
+ }
+ break;
+ case P2P_PS_ENABLE:
+ if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+ pwdinfo->p2p_ps_state = p2p_ps_state;
+
+ if (pwdinfo->ctwindow > 0) {
+ if (pwrpriv->smart_ps != 0) {
+ pwrpriv->smart_ps = 0;
+ DBG_88E("%s(): Enter CTW, change SmartPS\n", __func__);
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
+ }
+ }
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+ }
+ break;
+ case P2P_PS_SCAN:
+ case P2P_PS_SCAN_DONE:
+ case P2P_PS_ALLSTASLEEP:
+ if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+ pwdinfo->p2p_ps_state = p2p_ps_state;
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+ }
+ break;
+ default:
+ break;
+ }
+
+}
+
+u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
+{
+ struct cmd_obj *ph2c;
+ struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+ struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+ u8 res = _SUCCESS;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return res;
+
+ if (enqueue) {
+ ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+ if (ph2c == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+ if (pdrvextra_cmd_parm == NULL) {
+ kfree(ph2c);
+ res = _FAIL;
+ goto exit;
+ }
+
+ pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
+ pdrvextra_cmd_parm->type_size = p2p_ps_state;
+ pdrvextra_cmd_parm->pbuf = NULL;
+
+ init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+ res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+ } else {
+ p2p_ps_wk_hdl(padapter, p2p_ps_state);
+ }
+
+exit:
+
+ return res;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void reset_ch_sitesurvey_timer_process(struct timer_list *t)
+#else
+static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, pwrctrlpriv.pwr_state_check_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+
+ DBG_88E("[%s] In\n", __func__);
+ /* Reset the operation channel information */
+ pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+ pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void reset_ch_sitesurvey_timer_process2 (struct timer_list *t)
+#else
+static void reset_ch_sitesurvey_timer_process2 (void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, pwrctrlpriv.pwr_state_check_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+
+ DBG_88E("[%s] In\n", __func__);
+ /* Reset the operation channel information */
+ pwdinfo->p2p_info.operation_ch[0] = 0;
+ pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void restore_p2p_state_timer_process(struct timer_list *t)
+#else
+static void restore_p2p_state_timer_process (void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, wdinfo.restore_p2p_state_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+
+ p2p_protocol_wk_cmd(adapter, P2P_RESTORE_STATE_WK);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void pre_tx_scan_timer_process(struct timer_list *t)
+#else
+static void pre_tx_scan_timer_process(void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, wdinfo.pre_tx_scan_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+
+ spin_lock_bh(&pmlmepriv->lock);
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+ if (pwdinfo->tx_prov_disc_info.benable) { /* the provision discovery request frame is trigger to send or not */
+ p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
+ /* issue_probereq_p2p(adapter, NULL); */
+ /* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
+ }
+ } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+ if (pwdinfo->nego_req_info.benable)
+ p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
+ } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
+ if (pwdinfo->invitereq_info.benable)
+ p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
+ } else {
+ DBG_88E("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
+ }
+
+ spin_unlock_bh(&pmlmepriv->lock);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void find_phase_timer_process(struct timer_list *t)
+#else
+static void find_phase_timer_process(void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, wdinfo.find_phase_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+
+ if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+
+ adapter->wdinfo.find_phase_state_exchange_cnt++;
+
+ p2p_protocol_wk_cmd(adapter, P2P_FIND_PHASE_WK);
+}
+
+void reset_global_wifidirect_info(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo;
+
+ pwdinfo = &padapter->wdinfo;
+ pwdinfo->persistent_supported = 0;
+ pwdinfo->session_available = true;
+ pwdinfo->wfd_tdls_enable = 0;
+ pwdinfo->wfd_tdls_weaksec = 0;
+}
+
+void rtw_init_wifidirect_timers(struct adapter *padapter)
+{
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ timer_setup(&pwdinfo->find_phase_timer, find_phase_timer_process, 0);
+ timer_setup(&pwdinfo->restore_p2p_state_timer, restore_p2p_state_timer_process, 0);
+ timer_setup(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process, 0);
+ timer_setup(&pwdinfo->reset_ch_sitesurvey, reset_ch_sitesurvey_timer_process, 0);
+ timer_setup(&pwdinfo->reset_ch_sitesurvey2, reset_ch_sitesurvey_timer_process2, 0);
+#else
+ _init_timer(&pwdinfo->find_phase_timer, padapter->pnetdev, find_phase_timer_process, padapter);
+ _init_timer(&pwdinfo->restore_p2p_state_timer, padapter->pnetdev, restore_p2p_state_timer_process, padapter);
+ _init_timer(&pwdinfo->pre_tx_scan_timer, padapter->pnetdev, pre_tx_scan_timer_process, padapter);
+ _init_timer(&pwdinfo->reset_ch_sitesurvey, padapter->pnetdev, reset_ch_sitesurvey_timer_process, padapter);
+ _init_timer(&pwdinfo->reset_ch_sitesurvey2, padapter->pnetdev, reset_ch_sitesurvey_timer_process2, padapter);
+#endif
+}
+
+void rtw_init_wifidirect_addrs(struct adapter *padapter, u8 *dev_addr, u8 *iface_addr)
+{
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+ /*init device&interface address */
+ if (dev_addr)
+ memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
+ if (iface_addr)
+ memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
+#endif
+}
+
+void init_wifidirect_info(struct adapter *padapter, enum P2P_ROLE role)
+{
+ struct wifidirect_info *pwdinfo;
+
+ pwdinfo = &padapter->wdinfo;
+ pwdinfo->padapter = padapter;
+
+ /* 1, 6, 11 are the social channel defined in the WiFi Direct specification. */
+ pwdinfo->social_chan[0] = 1;
+ pwdinfo->social_chan[1] = 6;
+ pwdinfo->social_chan[2] = 11;
+ pwdinfo->social_chan[3] = 0; /* channel 0 for scanning ending in site survey function. */
+
+ /* Use the channel 11 as the listen channel */
+ pwdinfo->listen_channel = 11;
+
+ if (role == P2P_ROLE_DEVICE) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+ pwdinfo->intent = 1;
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
+ } else if (role == P2P_ROLE_CLIENT) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ pwdinfo->intent = 1;
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ } else if (role == P2P_ROLE_GO) {
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ pwdinfo->intent = 15;
+ rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+ }
+
+/* Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
+ pwdinfo->support_rate[0] = 0x8c; /* 6(B) */
+ pwdinfo->support_rate[1] = 0x92; /* 9(B) */
+ pwdinfo->support_rate[2] = 0x18; /* 12 */
+ pwdinfo->support_rate[3] = 0x24; /* 18 */
+ pwdinfo->support_rate[4] = 0x30; /* 24 */
+ pwdinfo->support_rate[5] = 0x48; /* 36 */
+ pwdinfo->support_rate[6] = 0x60; /* 48 */
+ pwdinfo->support_rate[7] = 0x6c; /* 54 */
+
+ memcpy(pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
+
+ memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
+ pwdinfo->device_name_len = 0;
+
+ memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
+ pwdinfo->invitereq_info.token = 3; /* Token used for P2P invitation request frame. */
+
+ memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
+ pwdinfo->inviteresp_info.token = 0;
+
+ pwdinfo->profileindex = 0;
+ memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
+
+ rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+
+ pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1);
+
+ memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
+ pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
+
+ memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
+
+ pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
+ pwdinfo->negotiation_dialog_token = 1;
+
+ memset(pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN);
+ pwdinfo->nego_ssidlen = 0;
+
+ pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+ pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
+ pwdinfo->channel_list_attr_len = 0;
+ memset(pwdinfo->channel_list_attr, 0x00, 100);
+
+ memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
+ memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
+ memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+ pwdinfo->wfd_tdls_enable = 0;
+ memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+ memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
+
+ pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+ pwdinfo->rx_invitereq_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */
+ pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+ pwdinfo->p2p_info.operation_ch[0] = 0;
+ pwdinfo->p2p_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */
+ pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+int rtw_p2p_enable(struct adapter *padapter, enum P2P_ROLE role)
+{
+ int ret = _SUCCESS;
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+ if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) {
+ /* leave IPS/Autosuspend */
+ if (_FAIL == rtw_pwr_wakeup(padapter)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* Added by Albert 2011/03/22 */
+ /* In the P2P mode, the driver should not support the b mode. */
+ /* So, the Tx packet shouldn't use the CCK rate */
+ update_tx_basic_rate(padapter, WIRELESS_11AGN);
+
+ /* Enable P2P function */
+ init_wifidirect_info(padapter, role);
+
+ rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, true);
+ } else if (role == P2P_ROLE_DISABLE) {
+ if (_FAIL == rtw_pwr_wakeup(padapter)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* Disable P2P function */
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+ _cancel_timer_ex(&pwdinfo->find_phase_timer);
+ _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+ _cancel_timer_ex(&pwdinfo->pre_tx_scan_timer);
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+ _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey2);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+#else
+ reset_ch_sitesurvey_timer_process(padapter);
+ reset_ch_sitesurvey_timer_process2(padapter);
+#endif
+ rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
+ rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
+ memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
+ }
+
+ rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, false);
+
+ /* Restore to initial setting. */
+ update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+ }
+
+exit:
+ return ret;
+}
+
+#else
+u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
+{
+ return _FAIL;
+}
+
+void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
+{
+}
+
+#endif /* CONFIG_88EU_P2P */
diff --git a/drivers/staging/r8188eu/core/rtw_pwrctrl.c b/drivers/staging/r8188eu/core/rtw_pwrctrl.c
new file mode 100644
index 000000000000..50e8b5e6aed8
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_pwrctrl.c
@@ -0,0 +1,655 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_PWRCTRL_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <osdep_intf.h>
+#include <linux/usb.h>
+
+void ips_enter(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct xmit_priv *pxmit_priv = &padapter->xmitpriv;
+
+ if (padapter->registrypriv.mp_mode == 1)
+ return;
+
+ if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+ pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
+ DBG_88E_LEVEL(_drv_info_, "There are some pkts to transmit\n");
+ DBG_88E_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
+ pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
+ return;
+ }
+
+ _enter_pwrlock(&pwrpriv->lock);
+
+ pwrpriv->bips_processing = true;
+
+ /* syn ips_mode with request */
+ pwrpriv->ips_mode = pwrpriv->ips_mode_req;
+
+ pwrpriv->ips_enter_cnts++;
+ DBG_88E("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);
+ if (rf_off == pwrpriv->change_rfpwrstate) {
+ pwrpriv->bpower_saving = true;
+ DBG_88E_LEVEL(_drv_info_, "nolinked power save enter\n");
+
+ if (pwrpriv->ips_mode == IPS_LEVEL_2)
+ pwrpriv->bkeepfwalive = true;
+
+ rtw_ips_pwr_down(padapter);
+ pwrpriv->rf_pwrstate = rf_off;
+ }
+ pwrpriv->bips_processing = false;
+
+ _exit_pwrlock(&pwrpriv->lock);
+}
+
+int ips_leave(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct security_priv *psecuritypriv = &(padapter->securitypriv);
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ int result = _SUCCESS;
+ int keyid;
+
+ _enter_pwrlock(&pwrpriv->lock);
+
+ if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) {
+ pwrpriv->bips_processing = true;
+ pwrpriv->change_rfpwrstate = rf_on;
+ pwrpriv->ips_leave_cnts++;
+ DBG_88E("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);
+
+ result = rtw_ips_pwr_up(padapter);
+ if (result == _SUCCESS) {
+ pwrpriv->rf_pwrstate = rf_on;
+ }
+ DBG_88E_LEVEL(_drv_info_, "nolinked power save leave\n");
+
+ if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) || (_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm)) {
+ DBG_88E("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing);
+ set_channel_bwmode(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+ for (keyid = 0; keyid < 4; keyid++) {
+ if (pmlmepriv->key_mask & BIT(keyid)) {
+ if (keyid == psecuritypriv->dot11PrivacyKeyIndex)
+ result = rtw_set_key(padapter, psecuritypriv, keyid, 1);
+ else
+ result = rtw_set_key(padapter, psecuritypriv, keyid, 0);
+ }
+ }
+ }
+
+ DBG_88E("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
+ pwrpriv->bips_processing = false;
+
+ pwrpriv->bkeepfwalive = false;
+ pwrpriv->bpower_saving = false;
+ }
+
+ _exit_pwrlock(&pwrpriv->lock);
+
+ return result;
+}
+
+static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
+{
+ struct adapter *buddy = adapter->pbuddy_adapter;
+ struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(adapter->wdinfo);
+#endif
+
+ bool ret = false;
+
+ if (adapter->pwrctrlpriv.ips_deny_time >= jiffies)
+ goto exit;
+
+ if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+ check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+ check_fwstate(pmlmepriv, WIFI_UNDER_WPS) ||
+ check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE) ||
+#if defined(CONFIG_88EU_P2P)
+ !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+#else
+ 0)
+#endif
+ goto exit;
+
+ /* consider buddy, if exist */
+ if (buddy) {
+ struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
+ #ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *b_pwdinfo = &(buddy->wdinfo);
+ #endif
+
+ if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+ check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+ check_fwstate(b_pmlmepriv, WIFI_AP_STATE) ||
+ check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE) ||
+#if defined(CONFIG_88EU_P2P)
+ !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE))
+#else
+ 0)
+#endif
+ goto exit;
+ }
+ ret = true;
+
+exit:
+ return ret;
+}
+
+void rtw_ps_processor(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ enum rt_rf_power_state rfpwrstate;
+
+ pwrpriv->ps_processing = true;
+
+ if (pwrpriv->bips_processing)
+ goto exit;
+
+ if (padapter->pwrctrlpriv.bHWPwrPindetect) {
+ rfpwrstate = RfOnOffDetect(padapter);
+ DBG_88E("@@@@- #2 %s==> rfstate:%s\n", __func__, (rfpwrstate == rf_on) ? "rf_on" : "rf_off");
+
+ if (rfpwrstate != pwrpriv->rf_pwrstate) {
+ if (rfpwrstate == rf_off) {
+ pwrpriv->change_rfpwrstate = rf_off;
+ pwrpriv->brfoffbyhw = true;
+ padapter->bCardDisableWOHSM = true;
+ rtw_hw_suspend(padapter);
+ } else {
+ pwrpriv->change_rfpwrstate = rf_on;
+ rtw_hw_resume(padapter);
+ }
+ DBG_88E("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off) ? "rf_off" : "rf_on");
+ }
+ pwrpriv->pwr_state_check_cnts++;
+ }
+
+ if (pwrpriv->ips_mode_req == IPS_NONE)
+ goto exit;
+
+ if (!rtw_pwr_unassociated_idle(padapter))
+ goto exit;
+
+ if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0)) {
+ DBG_88E("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv));
+ pwrpriv->change_rfpwrstate = rf_off;
+
+ ips_enter(padapter);
+ }
+exit:
+ rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
+ pwrpriv->ps_processing = false;
+ return;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+static void pwr_state_check_handler(struct timer_list *t)
+#else
+static void pwr_state_check_handler(void *FunctionContext)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *padapter =
+ from_timer(padapter, t,
+ pwrctrlpriv.pwr_state_check_timer);
+#else
+ struct adapter *padapter = (struct adapter *)FunctionContext;
+#endif
+ rtw_ps_cmd(padapter);
+}
+
+/*
+ *
+ * Parameters
+ * padapter
+ * pslv power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *
+ */
+void rtw_set_rpwm(struct adapter *padapter, u8 pslv)
+{
+ u8 rpwm;
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+ pslv = PS_STATE(pslv);
+
+ if (pwrpriv->btcoex_rfon) {
+ if (pslv < PS_STATE_S4)
+ pslv = PS_STATE_S3;
+ }
+
+ if ((pwrpriv->rpwm == pslv)) {
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+ ("%s: Already set rpwm[0x%02X], new=0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
+ return;
+ }
+
+ if ((padapter->bSurpriseRemoved) ||
+ (!padapter->hw_init_completed)) {
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+ ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+ __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+
+ pwrpriv->cpwm = PS_STATE_S4;
+
+ return;
+ }
+
+ if (padapter->bDriverStopped) {
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+ ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
+
+ if (pslv < PS_STATE_S2) {
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+ ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
+ return;
+ }
+ }
+
+ rpwm = pslv | pwrpriv->tog;
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+ ("rtw_set_rpwm: rpwm=0x%02x cpwm=0x%02x\n", rpwm, pwrpriv->cpwm));
+
+ pwrpriv->rpwm = pslv;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+
+ pwrpriv->tog += 0x80;
+ pwrpriv->cpwm = pslv;
+
+}
+
+static u8 PS_RDY_CHECK(struct adapter *padapter)
+{
+ u32 curr_time, delta_time;
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+ curr_time = jiffies;
+ delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp;
+
+ if (delta_time < LPS_DELAY_TIME)
+ return false;
+
+ if ((check_fwstate(pmlmepriv, _FW_LINKED) == false) ||
+ (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) ||
+ (check_fwstate(pmlmepriv, WIFI_AP_STATE)) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+ return false;
+ if (pwrpriv->bInSuspend)
+ return false;
+ if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false)) {
+ DBG_88E("Group handshake still in progress !!!\n");
+ return false;
+ }
+ return true;
+}
+
+void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+ ("%s: PowerMode=%d Smart_PS=%d\n",
+ __func__, ps_mode, smart_ps));
+
+ if (ps_mode > PM_Card_Disable) {
+ RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
+ return;
+ }
+
+ if (pwrpriv->pwr_mode == ps_mode) {
+ if (PS_MODE_ACTIVE == ps_mode)
+ return;
+
+ if ((pwrpriv->smart_ps == smart_ps) &&
+ (pwrpriv->bcn_ant_mode == bcn_ant_mode))
+ return;
+ }
+
+ /* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
+ if (ps_mode == PS_MODE_ACTIVE) {
+#ifdef CONFIG_88EU_P2P
+ if (pwdinfo->opp_ps == 0) {
+ DBG_88E("rtw_set_ps_mode: Leave 802.11 power save\n");
+ pwrpriv->pwr_mode = ps_mode;
+ rtw_set_rpwm(padapter, PS_STATE_S4);
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+ pwrpriv->bFwCurrentInPSMode = false;
+ }
+ } else {
+#endif /* CONFIG_88EU_P2P */
+ if (PS_RDY_CHECK(padapter)) {
+ DBG_88E("%s: Enter 802.11 power save\n", __func__);
+ pwrpriv->bFwCurrentInPSMode = true;
+ pwrpriv->pwr_mode = ps_mode;
+ pwrpriv->smart_ps = smart_ps;
+ pwrpriv->bcn_ant_mode = bcn_ant_mode;
+ rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+
+#ifdef CONFIG_88EU_P2P
+ /* Set CTWindow after LPS */
+ if (pwdinfo->opp_ps == 1)
+ p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0);
+#endif /* CONFIG_88EU_P2P */
+
+ rtw_set_rpwm(padapter, PS_STATE_S2);
+ }
+ }
+
+}
+
+/*
+ * Return:
+ * 0: Leave OK
+ * -1: Timeout
+ * -2: Other error
+ */
+s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
+{
+ u32 start_time;
+ u8 bAwake = false;
+ s32 err = 0;
+
+ start_time = jiffies;
+ while (1) {
+ rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
+ if (bAwake)
+ break;
+
+ if (padapter->bSurpriseRemoved) {
+ err = -2;
+ DBG_88E("%s: device surprise removed!!\n", __func__);
+ break;
+ }
+
+ if (rtw_get_passing_time_ms(start_time) > delay_ms) {
+ err = -1;
+ DBG_88E("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+ break;
+ }
+ rtw_usleep_os(100);
+ }
+
+ return err;
+}
+
+/* */
+/* Description: */
+/* Enter the leisure power save mode. */
+/* */
+void LPS_Enter(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+ if (PS_RDY_CHECK(padapter) == false)
+ return;
+
+ if (pwrpriv->bLeisurePs) {
+ /* Idle for a while if we connect to AP a while ago. */
+ if (pwrpriv->LpsIdleCount >= 2) { /* 4 Sec */
+ if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+ pwrpriv->bpower_saving = true;
+ DBG_88E("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps);
+ /* For Tenda W311R IOT issue */
+ rtw_set_ps_mode(padapter, pwrpriv->power_mgnt,
+ pwrpriv->smart_ps, 0x40);
+ }
+ } else {
+ pwrpriv->LpsIdleCount++;
+ }
+ }
+
+}
+
+#define LPS_LEAVE_TIMEOUT_MS 100
+
+/* Description: */
+/* Leave the leisure power save mode. */
+void LPS_Leave(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+ if (pwrpriv->bLeisurePs) {
+ if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+ rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0x40);
+
+ if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+ LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
+ }
+ }
+
+ pwrpriv->bpower_saving = false;
+
+}
+
+/* */
+/* Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */
+/* Move code to function by tynli. 2010.03.26. */
+/* */
+void LeaveAllPowerSaveMode(struct adapter *Adapter)
+{
+ struct mlme_priv *pmlmepriv = &(Adapter->mlmepriv);
+ u8 enqueue = 0;
+
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) { /* connect */
+ p2p_ps_wk_cmd(Adapter, P2P_PS_DISABLE, enqueue);
+
+ rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+ }
+
+}
+
+void rtw_init_pwrctrl_priv(struct adapter *padapter)
+{
+ struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+ _init_pwrlock(&pwrctrlpriv->lock);
+ pwrctrlpriv->rf_pwrstate = rf_on;
+ pwrctrlpriv->ips_enter_cnts = 0;
+ pwrctrlpriv->ips_leave_cnts = 0;
+ pwrctrlpriv->bips_processing = false;
+
+ pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+ pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+ pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+ pwrctrlpriv->pwr_state_check_cnts = 0;
+ pwrctrlpriv->bInternalAutoSuspend = false;
+ pwrctrlpriv->bInSuspend = false;
+ pwrctrlpriv->bkeepfwalive = false;
+
+ pwrctrlpriv->LpsIdleCount = 0;
+ if (padapter->registrypriv.mp_mode == 1)
+ pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE ;
+ else
+ pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/* PS_MODE_MIN; */
+ pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? true : false;
+
+ pwrctrlpriv->bFwCurrentInPSMode = false;
+
+ pwrctrlpriv->rpwm = 0;
+ pwrctrlpriv->cpwm = PS_STATE_S4;
+
+ pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+ pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
+ pwrctrlpriv->bcn_ant_mode = 0;
+
+ pwrctrlpriv->tog = 0x80;
+
+ pwrctrlpriv->btcoex_rfon = false;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ timer_setup(&pwrctrlpriv->pwr_state_check_timer, pwr_state_check_handler, 0);
+#else
+ _init_timer(&(pwrctrlpriv->pwr_state_check_timer), padapter->pnetdev, pwr_state_check_handler, (u8 *)padapter);
+#endif
+}
+
+void rtw_free_pwrctrl_priv(struct adapter *adapter)
+{
+ struct pwrctrl_priv *pwrctrlpriv = &adapter->pwrctrlpriv;
+
+ _free_pwrlock(&pwrctrlpriv->lock);
+
+}
+
+u8 rtw_interface_ps_func(struct adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val)
+{
+ u8 bResult = true;
+ rtw_hal_intf_ps_func(padapter, efunc_id, val);
+
+ return bResult;
+}
+
+inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ms);
+}
+
+/*
+* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+* @adapter: pointer to struct adapter structure
+* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup
+* Return _SUCCESS or _FAIL
+*/
+
+int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller)
+{
+ struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ int ret = _SUCCESS;
+ u32 start = jiffies;
+
+ if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+ pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+
+ if (pwrpriv->ps_processing) {
+ DBG_88E("%s wait ps_processing...\n", __func__);
+ while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
+ rtw_msleep_os(10);
+ if (pwrpriv->ps_processing)
+ DBG_88E("%s wait ps_processing timeout\n", __func__);
+ else
+ DBG_88E("%s wait ps_processing done\n", __func__);
+ }
+
+ /* System suspend is not allowed to wakeup */
+ if ((!pwrpriv->bInternalAutoSuspend) && pwrpriv->bInSuspend) {
+ while (pwrpriv->bInSuspend &&
+ (rtw_get_passing_time_ms(start) <= 3000 ||
+ (rtw_get_passing_time_ms(start) <= 500)))
+ rtw_msleep_os(10);
+ if (pwrpriv->bInSuspend)
+ DBG_88E("%s wait bInSuspend timeout\n", __func__);
+ else
+ DBG_88E("%s wait bInSuspend done\n", __func__);
+ }
+
+ /* block??? */
+ if ((pwrpriv->bInternalAutoSuspend) && (padapter->net_closed)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* I think this should be check in IPS, LPS, autosuspend functions... */
+ if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+ ret = _SUCCESS;
+ goto exit;
+ }
+ if (rf_off == pwrpriv->rf_pwrstate) {
+ DBG_88E("%s call ips_leave....\n", __func__);
+ if (_FAIL == ips_leave(padapter)) {
+ DBG_88E("======> ips_leave fail.............\n");
+ ret = _FAIL;
+ goto exit;
+ }
+ }
+
+ /* TODO: the following checking need to be merged... */
+ if (padapter->bDriverStopped || !padapter->bup ||
+ !padapter->hw_init_completed) {
+ DBG_88E("%s: bDriverStopped=%d, bup=%d, hw_init_completed =%u\n"
+ , caller
+ , padapter->bDriverStopped
+ , padapter->bup
+ , padapter->hw_init_completed);
+ ret = false;
+ goto exit;
+ }
+
+exit:
+ if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+ pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+ return ret;
+}
+
+int rtw_pm_set_lps(struct adapter *padapter, u8 mode)
+{
+ int ret = 0;
+ struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+ if (mode < PS_MODE_NUM) {
+ if (pwrctrlpriv->power_mgnt != mode) {
+ if (PS_MODE_ACTIVE == mode)
+ LeaveAllPowerSaveMode(padapter);
+ else
+ pwrctrlpriv->LpsIdleCount = 2;
+ pwrctrlpriv->power_mgnt = mode;
+ pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? true : false;
+ }
+ } else {
+ ret = -EINVAL;
+ }
+
+ return ret;
+}
+
+int rtw_pm_set_ips(struct adapter *padapter, u8 mode)
+{
+ struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+ if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
+ rtw_ips_mode_req(pwrctrlpriv, mode);
+ DBG_88E("%s %s\n", __func__, mode == IPS_NORMAL ? "IPS_NORMAL" : "IPS_LEVEL_2");
+ return 0;
+ } else if (mode == IPS_NONE) {
+ rtw_ips_mode_req(pwrctrlpriv, mode);
+ DBG_88E("%s %s\n", __func__, "IPS_NONE");
+ if ((padapter->bSurpriseRemoved == 0) && (_FAIL == rtw_pwr_wakeup(padapter)))
+ return -EFAULT;
+ } else {
+ return -EINVAL;
+ }
+ return 0;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_recv.c b/drivers/staging/r8188eu/core/rtw_recv.c
new file mode 100644
index 000000000000..16a38a2fbe87
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_recv.c
@@ -0,0 +1,2252 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_RECV_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <mlme_osdep.h>
+#include <ip.h>
+#include <if_ether.h>
+#include <ethernet.h>
+#include <usb_ops.h>
+#include <wifi.h>
+
+static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static u8 rtw_bridge_tunnel_header[] = {
+ 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8
+};
+
+static u8 rtw_rfc1042_header[] = {
+ 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+void rtw_signal_stat_timer_hdl(struct timer_list *);
+#else
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+#endif
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+
+ memset((u8 *)psta_recvpriv, 0, sizeof (struct sta_recv_priv));
+
+ spin_lock_init(&psta_recvpriv->lock);
+
+ _rtw_init_queue(&psta_recvpriv->defrag_q);
+
+}
+
+int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
+{
+ int i;
+
+ struct recv_frame *precvframe;
+
+ int res = _SUCCESS;
+
+ spin_lock_init(&precvpriv->lock);
+
+ _rtw_init_queue(&precvpriv->free_recv_queue);
+ _rtw_init_queue(&precvpriv->recv_pending_queue);
+ _rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
+
+ precvpriv->adapter = padapter;
+
+ precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+ rtw_os_recv_resource_init(precvpriv, padapter);
+
+ precvpriv->pallocated_frame_buf = rtw_zvmalloc(NR_RECVFRAME * sizeof(struct recv_frame) + RXFRAME_ALIGN_SZ);
+
+ if (precvpriv->pallocated_frame_buf == NULL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((size_t)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ);
+
+ precvframe = (struct recv_frame *)precvpriv->precv_frame_buf;
+
+ for (i = 0; i < NR_RECVFRAME; i++) {
+ INIT_LIST_HEAD(&(precvframe->list));
+
+ list_add_tail(&(precvframe->list), &(precvpriv->free_recv_queue.queue));
+
+ res = rtw_os_recv_resource_alloc(padapter, precvframe);
+
+ precvframe->len = 0;
+
+ precvframe->adapter = padapter;
+ precvframe++;
+ }
+ precvpriv->rx_pending_cnt = 1;
+
+ sema_init(&precvpriv->allrxreturnevt, 0);
+
+ res = rtw_hal_init_recv_priv(padapter);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ timer_setup(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl, 0);
+#else
+ _init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
+#endif
+ precvpriv->signal_stat_sampling_interval = 1000; /* ms */
+
+ rtw_set_signal_stat_timer(precvpriv);
+exit:
+
+ return res;
+}
+
+static void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv)
+{
+ _rtw_spinlock_free(&precvpriv->lock);
+ _rtw_spinlock_free(&precvpriv->free_recv_queue.lock);
+ _rtw_spinlock_free(&precvpriv->recv_pending_queue.lock);
+
+ _rtw_spinlock_free(&precvpriv->free_recv_buf_queue.lock);
+}
+
+void _rtw_free_recv_priv (struct recv_priv *precvpriv)
+{
+ struct adapter *padapter = precvpriv->adapter;
+
+ rtw_free_uc_swdec_pending_queue(padapter);
+
+ rtw_mfree_recv_priv_lock(precvpriv);
+
+ rtw_os_recv_resource_free(precvpriv);
+
+ if (precvpriv->pallocated_frame_buf) {
+ rtw_vmfree(precvpriv->pallocated_frame_buf, NR_RECVFRAME * sizeof(struct recv_frame) + RXFRAME_ALIGN_SZ);
+ }
+
+ rtw_hal_free_recv_priv(padapter);
+
+}
+
+struct recv_frame *_rtw_alloc_recvframe (struct __queue *pfree_recv_queue)
+{
+ struct recv_frame *hdr;
+ struct list_head *plist, *phead;
+ struct adapter *padapter;
+ struct recv_priv *precvpriv;
+
+ if (list_empty(&pfree_recv_queue->queue)) {
+ hdr = NULL;
+ } else {
+ phead = get_list_head(pfree_recv_queue);
+
+ plist = phead->next;
+
+ hdr = container_of(plist, struct recv_frame, list);
+
+ list_del_init(&hdr->list);
+ padapter = hdr->adapter;
+ if (padapter != NULL) {
+ precvpriv = &padapter->recvpriv;
+ if (pfree_recv_queue == &precvpriv->free_recv_queue)
+ precvpriv->free_recvframe_cnt--;
+ }
+ }
+
+ return (struct recv_frame *)hdr;
+}
+
+struct recv_frame *rtw_alloc_recvframe (struct __queue *pfree_recv_queue)
+{
+ struct recv_frame *precvframe;
+
+ spin_lock_bh(&pfree_recv_queue->lock);
+
+ precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+ spin_unlock_bh(&pfree_recv_queue->lock);
+
+ return precvframe;
+}
+
+void rtw_init_recvframe(struct recv_frame *precvframe, struct recv_priv *precvpriv)
+{
+ /* Perry: This can be removed */
+ INIT_LIST_HEAD(&precvframe->list);
+
+ precvframe->len = 0;
+}
+
+int rtw_free_recvframe(struct recv_frame *precvframe, struct __queue *pfree_recv_queue)
+{
+ struct adapter *padapter;
+ struct recv_priv *precvpriv;
+
+ if (!precvframe)
+ return _FAIL;
+ padapter = precvframe->adapter;
+ precvpriv = &padapter->recvpriv;
+ if (precvframe->pkt) {
+ dev_kfree_skb_any(precvframe->pkt);/* free skb by driver */
+ precvframe->pkt = NULL;
+ }
+
+ spin_lock_bh(&pfree_recv_queue->lock);
+
+ list_del_init(&(precvframe->list));
+
+ precvframe->len = 0;
+
+ list_add_tail(&(precvframe->list), get_list_head(pfree_recv_queue));
+
+ if (padapter != NULL) {
+ if (pfree_recv_queue == &precvpriv->free_recv_queue)
+ precvpriv->free_recvframe_cnt++;
+ }
+
+ spin_unlock_bh(&pfree_recv_queue->lock);
+
+ return _SUCCESS;
+}
+
+int _rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue)
+{
+ struct adapter *padapter = precvframe->adapter;
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+
+ list_del_init(&(precvframe->list));
+ list_add_tail(&(precvframe->list), get_list_head(queue));
+
+ if (padapter != NULL) {
+ if (queue == &precvpriv->free_recv_queue)
+ precvpriv->free_recvframe_cnt++;
+ }
+
+ return _SUCCESS;
+}
+
+int rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue)
+{
+ int ret;
+
+ spin_lock_bh(&queue->lock);
+ ret = _rtw_enqueue_recvframe(precvframe, queue);
+ spin_unlock_bh(&queue->lock);
+
+ return ret;
+}
+
+/*
+caller : defrag ; recvframe_chk_defrag in recv_thread (passive)
+pframequeue: defrag_queue : will be accessed in recv_thread (passive)
+
+using spinlock to protect
+
+*/
+
+void rtw_free_recvframe_queue(struct __queue *pframequeue, struct __queue *pfree_recv_queue)
+{
+ struct recv_frame *hdr;
+ struct list_head *plist, *phead;
+
+ spin_lock(&pframequeue->lock);
+
+ phead = get_list_head(pframequeue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ hdr = container_of(plist, struct recv_frame, list);
+
+ plist = plist->next;
+
+ rtw_free_recvframe((struct recv_frame *)hdr, pfree_recv_queue);
+ }
+
+ spin_unlock(&pframequeue->lock);
+
+}
+
+u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter)
+{
+ u32 cnt = 0;
+ struct recv_frame *pending_frame;
+ while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) {
+ rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue);
+ DBG_88E("%s: dequeue uc_swdec_pending_queue\n", __func__);
+ cnt++;
+ }
+
+ return cnt;
+}
+
+int rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue)
+{
+ spin_lock_bh(&queue->lock);
+
+ list_del_init(&precvbuf->list);
+ list_add(&precvbuf->list, get_list_head(queue));
+
+ spin_unlock_bh(&queue->lock);
+
+ return _SUCCESS;
+}
+
+int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue)
+{
+ unsigned long flags;
+
+ spin_lock_irqsave(&queue->lock, flags);
+
+ list_del_init(&precvbuf->list);
+
+ list_add_tail(&precvbuf->list, get_list_head(queue));
+ spin_unlock_irqrestore(&queue->lock, flags);
+ return _SUCCESS;
+}
+
+struct recv_buf *rtw_dequeue_recvbuf (struct __queue *queue)
+{
+ struct recv_buf *precvbuf;
+ struct list_head *plist, *phead;
+ unsigned long flags;
+
+ spin_lock_irqsave(&queue->lock, flags);
+
+ if (list_empty(&queue->queue)) {
+ precvbuf = NULL;
+ } else {
+ phead = get_list_head(queue);
+
+ plist = phead->next;
+
+ precvbuf = container_of(plist, struct recv_buf, list);
+
+ list_del_init(&precvbuf->list);
+ }
+
+ spin_unlock_irqrestore(&queue->lock, flags);
+
+ return precvbuf;
+}
+
+static int recvframe_chkmic(struct adapter *adapter, struct recv_frame *precvframe)
+{
+ int i, res = _SUCCESS;
+ u32 datalen;
+ u8 miccode[8];
+ u8 bmic_err = false, brpt_micerror = true;
+ u8 *pframe, *payload, *pframemic;
+ u8 *mickey;
+ struct sta_info *stainfo;
+ struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+ struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ stainfo = rtw_get_stainfo(&adapter->stapriv, &prxattrib->ta[0]);
+
+ if (prxattrib->encrypt == _TKIP_) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:prxattrib->encrypt==_TKIP_\n"));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:da=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+ prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2], prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5]));
+
+ /* calculate mic code */
+ if (stainfo != NULL) {
+ if (IS_MCAST(prxattrib->ra)) {
+ mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic: bcmc key\n"));
+
+ if (!psecuritypriv) {
+ res = _FAIL;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"));
+ DBG_88E("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n");
+ goto exit;
+ }
+ } else {
+ mickey = &stainfo->dot11tkiprxmickey.skey[0];
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic: unicast key\n"));
+ }
+
+ datalen = precvframe->len-prxattrib->hdrlen-prxattrib->iv_len-prxattrib->icv_len-8;/* icv_len included the mic code */
+ pframe = precvframe->rx_data;
+ payload = pframe+prxattrib->hdrlen+prxattrib->iv_len;
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n prxattrib->iv_len=%d prxattrib->icv_len=%d\n", prxattrib->iv_len, prxattrib->icv_len));
+ rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0],
+ (unsigned char)prxattrib->priority); /* care the length of the data */
+
+ pframemic = payload+datalen;
+
+ bmic_err = false;
+
+ for (i = 0; i < 8; i++) {
+ if (miccode[i] != *(pframemic+i)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+ ("recvframe_chkmic:miccode[%d](%02x)!=*(pframemic+%d)(%02x) ",
+ i, miccode[i], i, *(pframemic+i)));
+ bmic_err = true;
+ }
+ }
+
+ if (bmic_err) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+ ("\n *(pframemic-8)-*(pframemic-1)=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+ *(pframemic-8), *(pframemic-7), *(pframemic-6),
+ *(pframemic-5), *(pframemic-4), *(pframemic-3),
+ *(pframemic-2), *(pframemic-1)));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+ ("\n *(pframemic-16)-*(pframemic-9)=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+ *(pframemic-16), *(pframemic-15), *(pframemic-14),
+ *(pframemic-13), *(pframemic-12), *(pframemic-11),
+ *(pframemic-10), *(pframemic-9)));
+ {
+ uint i;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet (len=%d)======\n", precvframe->len));
+ for (i = 0; i < precvframe->len; i = i+8) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x",
+ *(precvframe->rx_data+i), *(precvframe->rx_data+i+1),
+ *(precvframe->rx_data+i+2), *(precvframe->rx_data+i+3),
+ *(precvframe->rx_data+i+4), *(precvframe->rx_data+i+5),
+ *(precvframe->rx_data+i+6), *(precvframe->rx_data+i+7)));
+ }
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ====== demp packet end [len=%d]======\n", precvframe->len));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n hrdlen=%d,\n", prxattrib->hdrlen));
+ }
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+ ("ra=0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x psecuritypriv->binstallGrpkey=%d ",
+ prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2],
+ prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5], psecuritypriv->binstallGrpkey));
+
+ /* double check key_index for some timing issue , */
+ /* cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */
+ if ((IS_MCAST(prxattrib->ra) == true) && (prxattrib->key_index != pmlmeinfo->key_index))
+ brpt_micerror = false;
+
+ if ((prxattrib->bdecrypted) && (brpt_micerror)) {
+ rtw_handle_tkip_mic_err(adapter, (u8)IS_MCAST(prxattrib->ra));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted=%d ", prxattrib->bdecrypted));
+ DBG_88E(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted);
+ } else {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted=%d ", prxattrib->bdecrypted));
+ DBG_88E(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted);
+ }
+ res = _FAIL;
+ } else {
+ /* mic checked ok */
+ if ((!psecuritypriv->bcheck_grpkey) && (IS_MCAST(prxattrib->ra))) {
+ psecuritypriv->bcheck_grpkey = true;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("psecuritypriv->bcheck_grpkey = true"));
+ }
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic: rtw_get_stainfo==NULL!!!\n"));
+ }
+
+ recvframe_pull_tail(precvframe, 8);
+ }
+
+exit:
+
+ return res;
+}
+
+/* decrypt and set the ivlen, icvlen of the recv_frame */
+static struct recv_frame *decryptor(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ struct rx_pkt_attrib *prxattrib = &precv_frame->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct recv_frame *return_packet = precv_frame;
+ u32 res = _SUCCESS;
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("prxstat->decrypted=%x prxattrib->encrypt=0x%03x\n", prxattrib->bdecrypted, prxattrib->encrypt));
+
+ if (prxattrib->encrypt > 0) {
+ u8 *iv = precv_frame->rx_data+prxattrib->hdrlen;
+ prxattrib->key_index = (((iv[3])>>6)&0x3);
+
+ if (prxattrib->key_index > WEP_KEYS) {
+ DBG_88E("prxattrib->key_index(%d)>WEP_KEYS\n", prxattrib->key_index);
+
+ switch (prxattrib->encrypt) {
+ case _WEP40_:
+ case _WEP104_:
+ prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+ break;
+ case _TKIP_:
+ case _AES_:
+ default:
+ prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+ break;
+ }
+ }
+ }
+
+ if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt))) {
+ psecuritypriv->hw_decrypted = false;
+
+ switch (prxattrib->encrypt) {
+ case _WEP40_:
+ case _WEP104_:
+ rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+ break;
+ case _TKIP_:
+ res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame);
+ break;
+ case _AES_:
+ res = rtw_aes_decrypt(padapter, (u8 *)precv_frame);
+ break;
+ default:
+ break;
+ }
+ } else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
+ (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_))
+ psecuritypriv->hw_decrypted = true;
+
+ if (res == _FAIL) {
+ rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue);
+ return_packet = NULL;
+ } else {
+ prxattrib->bdecrypted = true;
+ }
+
+ return return_packet;
+}
+
+/* set the security information in the recv_frame */
+static struct recv_frame *portctrl(struct adapter *adapter, struct recv_frame *precv_frame)
+{
+ u8 *psta_addr, *ptr;
+ uint auth_alg;
+ struct recv_frame *pfhdr;
+ struct sta_info *psta;
+ struct sta_priv *pstapriv;
+ struct recv_frame *prtnframe;
+ u16 ether_type;
+ u16 eapol_type = 0x888e;/* for Funia BD's WPA issue */
+ struct rx_pkt_attrib *pattrib;
+ __be16 be_tmp;
+
+ pstapriv = &adapter->stapriv;
+
+ auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
+
+ ptr = precv_frame->rx_data;
+ pfhdr = precv_frame;
+ pattrib = &pfhdr->attrib;
+ psta_addr = pattrib->ta;
+
+ prtnframe = NULL;
+
+ psta = rtw_get_stainfo(pstapriv, psta_addr);
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+ ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm=%d\n",
+ adapter->securitypriv.dot11AuthAlgrthm));
+
+ if (auth_alg == 2) {
+ if ((psta != NULL) && (psta->ieee8021x_blocked)) {
+ /* blocked */
+ /* only accept EAPOL frame */
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked==1\n"));
+
+ prtnframe = precv_frame;
+
+ /* get ether_type */
+ ptr = ptr+pfhdr->attrib.hdrlen+pfhdr->attrib.iv_len+LLC_HEADER_SIZE;
+ memcpy(&be_tmp, ptr, 2);
+ ether_type = ntohs(be_tmp);
+
+ if (ether_type == eapol_type) {
+ prtnframe = precv_frame;
+ } else {
+ /* free this frame */
+ rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue);
+ prtnframe = NULL;
+ }
+ } else {
+ /* allowed */
+ /* check decryption status, and decrypt the frame if needed */
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked==0\n"));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:precv_frame->hdr.attrib.privacy=%x\n", precv_frame->attrib.privacy));
+
+ if (pattrib->bdecrypted == 0)
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:prxstat->decrypted=%x\n", pattrib->bdecrypted));
+
+ prtnframe = precv_frame;
+ /* check is the EAPOL frame or not (Rekey) */
+ if (ether_type == eapol_type) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("########portctrl:ether_type==0x888e\n"));
+ /* check Rekey */
+
+ prtnframe = precv_frame;
+ } else {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:ether_type=0x%04x\n", ether_type));
+ }
+ }
+ } else {
+ prtnframe = precv_frame;
+ }
+
+ return prtnframe;
+}
+
+static int recv_decache(struct recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+ int tid = precv_frame->attrib.priority;
+
+ u16 seq_ctrl = ((precv_frame->attrib.seq_num&0xffff) << 4) |
+ (precv_frame->attrib.frag_num & 0xf);
+
+ if (tid > 15) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, (tid>15)! seq_ctrl=0x%x, tid=0x%x\n", seq_ctrl, tid));
+
+ return _FAIL;
+ }
+
+ if (1) {/* if (bretry) */
+ if (seq_ctrl == prxcache->tid_rxseq[tid]) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, seq_ctrl=0x%x, tid=0x%x, tid_rxseq=0x%x\n", seq_ctrl, tid, prxcache->tid_rxseq[tid]));
+
+ return _FAIL;
+ }
+ }
+
+ prxcache->tid_rxseq[tid] = seq_ctrl;
+
+ return _SUCCESS;
+}
+
+void process_pwrbit_data(struct adapter *padapter, struct recv_frame *precv_frame);
+void process_pwrbit_data(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ unsigned char pwrbit;
+ u8 *ptr = precv_frame->rx_data;
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct sta_info *psta = NULL;
+
+ psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+ pwrbit = GetPwrMgt(ptr);
+
+ if (psta) {
+ if (pwrbit) {
+ if (!(psta->state & WIFI_SLEEP_STATE))
+ stop_sta_xmit(padapter, psta);
+ } else {
+ if (psta->state & WIFI_SLEEP_STATE)
+ wakeup_sta_to_xmit(padapter, psta);
+ }
+ }
+
+#endif
+}
+
+static void process_wmmps_data(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct sta_info *psta = NULL;
+
+ psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+ if (!psta)
+ return;
+
+ if (!psta->qos_option)
+ return;
+
+ if (!(psta->qos_info&0xf))
+ return;
+
+ if (psta->state&WIFI_SLEEP_STATE) {
+ u8 wmmps_ac = 0;
+
+ switch (pattrib->priority) {
+ case 1:
+ case 2:
+ wmmps_ac = psta->uapsd_bk&BIT(1);
+ break;
+ case 4:
+ case 5:
+ wmmps_ac = psta->uapsd_vi&BIT(1);
+ break;
+ case 6:
+ case 7:
+ wmmps_ac = psta->uapsd_vo&BIT(1);
+ break;
+ case 0:
+ case 3:
+ default:
+ wmmps_ac = psta->uapsd_be&BIT(1);
+ break;
+ }
+
+ if (wmmps_ac) {
+ if (psta->sleepq_ac_len > 0) {
+ /* process received triggered frame */
+ xmit_delivery_enabled_frames(padapter, psta);
+ } else {
+ /* issue one qos null frame with More data bit = 0 and the EOSP bit set (= 1) */
+ issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0);
+ }
+ }
+ }
+
+#endif
+}
+
+static void count_rx_stats(struct adapter *padapter, struct recv_frame *prframe, struct sta_info *sta)
+{
+ int sz;
+ struct sta_info *psta = NULL;
+ struct stainfo_stats *pstats = NULL;
+ struct rx_pkt_attrib *pattrib = &prframe->attrib;
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+
+ sz = get_recvframe_len(prframe);
+ precvpriv->rx_bytes += sz;
+
+ padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+ if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst)))
+ padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+
+ if (sta)
+ psta = sta;
+ else
+ psta = prframe->psta;
+
+ if (psta) {
+ pstats = &psta->sta_stats;
+
+ pstats->rx_data_pkts++;
+ pstats->rx_bytes += sz;
+ }
+}
+
+int sta2sta_data_frame(
+ struct adapter *adapter,
+ struct recv_frame *precv_frame,
+ struct sta_info **psta
+);
+
+int sta2sta_data_frame(struct adapter *adapter, struct recv_frame *precv_frame, struct sta_info **psta)
+{
+ u8 *ptr = precv_frame->rx_data;
+ int ret = _SUCCESS;
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ u8 *mybssid = get_bssid(pmlmepriv);
+ u8 *myhwaddr = myid(&adapter->eeprompriv);
+ u8 *sta_addr = NULL;
+ int bmcast = IS_MCAST(pattrib->dst);
+
+ if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
+ (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+ /* filter packets that SA is myself or multicast or broadcast */
+ if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA==myself\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+ !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+ memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ sta_addr = pattrib->src;
+ } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+ /* For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */
+ if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("bssid!=TA under STATION_MODE; drop pkt\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+ sta_addr = pattrib->bssid;
+ } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ if (bmcast) {
+ /* For AP mode, if DA == MCAST, then BSSID should be also MCAST */
+ if (!IS_MCAST(pattrib->bssid)) {
+ ret = _FAIL;
+ goto exit;
+ }
+ } else { /* not mc-frame */
+ /* For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */
+ if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ sta_addr = pattrib->src;
+ }
+ } else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+ memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+ memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+ sta_addr = mybssid;
+ } else {
+ ret = _FAIL;
+ }
+
+ if (bmcast)
+ *psta = rtw_get_bcmc_stainfo(adapter);
+ else
+ *psta = rtw_get_stainfo(pstapriv, sta_addr); /* get ap_info */
+
+ if (*psta == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under sta2sta_data_frame ; drop pkt\n"));
+ if (adapter->registrypriv.mp_mode == 1) {
+ if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)
+ adapter->mppriv.rx_pktloss++;
+ }
+ ret = _FAIL;
+ goto exit;
+ }
+
+exit:
+
+ return ret;
+}
+
+static int ap2sta_data_frame (
+ struct adapter *adapter,
+ struct recv_frame *precv_frame,
+ struct sta_info **psta)
+{
+ u8 *ptr = precv_frame->rx_data;
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ int ret = _SUCCESS;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ u8 *mybssid = get_bssid(pmlmepriv);
+ u8 *myhwaddr = myid(&adapter->eeprompriv);
+ int bmcast = IS_MCAST(pattrib->dst);
+
+ if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) &&
+ (check_fwstate(pmlmepriv, _FW_LINKED) == true ||
+ check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
+ /* filter packets that SA is myself or multicast or broadcast */
+ if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA==myself\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* da should be for me */
+ if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+ (" ap2sta_data_frame: compare DA fail; DA=%pM\n", (pattrib->dst)));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* check BSSID */
+ if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+ !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+ (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+ (" ap2sta_data_frame: compare BSSID fail ; BSSID=%pM\n", (pattrib->bssid)));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("mybssid=%pM\n", (mybssid)));
+
+ if (!bmcast) {
+ DBG_88E("issue_deauth to the nonassociated ap=%pM for the reason(7)\n", (pattrib->bssid));
+ issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+ }
+
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if (bmcast)
+ *psta = rtw_get_bcmc_stainfo(adapter);
+ else
+ *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get ap_info */
+
+ if (*psta == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ap2sta: can't get psta under STATION_MODE ; drop pkt\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) { */
+ /* */
+
+ if (GetFrameSubType(ptr) & BIT(6)) {
+ /* No data, will not indicate to upper layer, temporily count it here */
+ count_rx_stats(adapter, precv_frame, *psta);
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ }
+ } else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
+ (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+ memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+ memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+ /* */
+ memcpy(pattrib->bssid, mybssid, ETH_ALEN);
+
+ *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */
+ if (*psta == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under MP_MODE ; drop pkt\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+ } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ /* Special case */
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ } else {
+ if (!memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) {
+ *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */
+ if (*psta == NULL) {
+ DBG_88E("issue_deauth to the ap =%pM for the reason(7)\n", (pattrib->bssid));
+
+ issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+ }
+ }
+
+ ret = _FAIL;
+ }
+
+exit:
+
+ return ret;
+}
+
+static int sta2ap_data_frame(struct adapter *adapter,
+ struct recv_frame *precv_frame,
+ struct sta_info **psta)
+{
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct sta_priv *pstapriv = &adapter->stapriv;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+ u8 *ptr = precv_frame->rx_data;
+ unsigned char *mybssid = get_bssid(pmlmepriv);
+ int ret = _SUCCESS;
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+ /* For AP mode, RA = BSSID, TX = STA(SRC_ADDR), A3 = DST_ADDR */
+ if (memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ *psta = rtw_get_stainfo(pstapriv, pattrib->src);
+ if (*psta == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under AP_MODE; drop pkt\n"));
+ DBG_88E("issue_deauth to sta=%pM for the reason(7)\n", (pattrib->src));
+
+ issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ }
+
+ process_pwrbit_data(adapter, precv_frame);
+
+ if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+ process_wmmps_data(adapter, precv_frame);
+ }
+
+ if (GetFrameSubType(ptr) & BIT(6)) {
+ /* No data, will not indicate to upper layer, temporily count it here */
+ count_rx_stats(adapter, precv_frame, *psta);
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ }
+ } else {
+ u8 *myhwaddr = myid(&adapter->eeprompriv);
+ if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ }
+ DBG_88E("issue_deauth to sta=%pM for the reason(7)\n", (pattrib->src));
+ issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+ ret = RTW_RX_HANDLED;
+ goto exit;
+ }
+
+exit:
+
+ return ret;
+}
+
+static int validate_recv_ctrl_frame(struct adapter *padapter,
+ struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 *pframe = precv_frame->rx_data;
+ /* uint len = precv_frame->len; */
+
+ if (GetFrameType(pframe) != WIFI_CTRL_TYPE)
+ return _FAIL;
+
+ /* receive the frames that ra(a1) is my address */
+ if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN))
+ return _FAIL;
+
+ /* only handle ps-poll */
+ if (GetFrameSubType(pframe) == WIFI_PSPOLL) {
+ u16 aid;
+ u8 wmmps_ac = 0;
+ struct sta_info *psta = NULL;
+
+ aid = GetAid(pframe);
+ psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+ if ((psta == NULL) || (psta->aid != aid))
+ return _FAIL;
+
+ /* for rx pkt statistics */
+ psta->sta_stats.rx_ctrl_pkts++;
+
+ switch (pattrib->priority) {
+ case 1:
+ case 2:
+ wmmps_ac = psta->uapsd_bk&BIT(0);
+ break;
+ case 4:
+ case 5:
+ wmmps_ac = psta->uapsd_vi&BIT(0);
+ break;
+ case 6:
+ case 7:
+ wmmps_ac = psta->uapsd_vo&BIT(0);
+ break;
+ case 0:
+ case 3:
+ default:
+ wmmps_ac = psta->uapsd_be&BIT(0);
+ break;
+ }
+
+ if (wmmps_ac)
+ return _FAIL;
+
+ if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+ DBG_88E("%s alive check-rx ps-poll\n", __func__);
+ psta->expire_to = pstapriv->expire_to;
+ psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+ }
+
+ if ((psta->state&WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap&BIT(psta->aid))) {
+ struct list_head *xmitframe_plist, *xmitframe_phead;
+ struct xmit_frame *pxmitframe = NULL;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ spin_lock_bh(&pxmitpriv->lock);
+
+ xmitframe_phead = get_list_head(&psta->sleep_q);
+ xmitframe_plist = xmitframe_phead->next;
+
+ if (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ list_del_init(&pxmitframe->list);
+
+ psta->sleepq_len--;
+
+ if (psta->sleepq_len > 0)
+ pxmitframe->attrib.mdata = 1;
+ else
+ pxmitframe->attrib.mdata = 0;
+
+ pxmitframe->attrib.triggered = 1;
+
+ rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+ if (psta->sleepq_len == 0) {
+ pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+ /* upate BCN for TIM IE */
+ /* update_BCNTIM(padapter); */
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+ }
+ } else {
+ if (pstapriv->tim_bitmap&BIT(psta->aid)) {
+ if (psta->sleepq_len == 0) {
+ DBG_88E("no buffered packets to xmit\n");
+
+ /* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+ issue_nulldata(padapter, psta->hwaddr, 0, 0, 0);
+ } else {
+ DBG_88E("error!psta->sleepq_len=%d\n", psta->sleepq_len);
+ psta->sleepq_len = 0;
+ }
+
+ pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+ /* upate BCN for TIM IE */
+ /* update_BCNTIM(padapter); */
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+ }
+ }
+ spin_unlock_bh(&pxmitpriv->lock);
+ }
+ }
+
+#endif
+
+ return _FAIL;
+}
+
+struct recv_frame *recvframe_chk_defrag(struct adapter *padapter, struct recv_frame *precv_frame);
+
+static int validate_recv_mgnt_frame(struct adapter *padapter,
+ struct recv_frame *precv_frame)
+{
+ struct sta_info *psta;
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+validate_recv_mgnt_frame\n"));
+
+ precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+ if (precv_frame == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("%s: fragment packet\n", __func__));
+ return _SUCCESS;
+ }
+
+ /* for rx pkt statistics */
+ psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(precv_frame->rx_data));
+ if (psta) {
+ psta->sta_stats.rx_mgnt_pkts++;
+ if (GetFrameSubType(precv_frame->rx_data) == WIFI_BEACON) {
+ psta->sta_stats.rx_beacon_pkts++;
+ } else if (GetFrameSubType(precv_frame->rx_data) == WIFI_PROBEREQ) {
+ psta->sta_stats.rx_probereq_pkts++;
+ } else if (GetFrameSubType(precv_frame->rx_data) == WIFI_PROBERSP) {
+ if (!memcmp(padapter->eeprompriv.mac_addr, GetAddr1Ptr(precv_frame->rx_data), ETH_ALEN))
+ psta->sta_stats.rx_probersp_pkts++;
+ else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->rx_data)) ||
+ is_multicast_mac_addr(GetAddr1Ptr(precv_frame->rx_data)))
+ psta->sta_stats.rx_probersp_bm_pkts++;
+ else
+ psta->sta_stats.rx_probersp_uo_pkts++;
+ }
+ }
+
+ mgt_dispatcher(padapter, precv_frame);
+
+ return _SUCCESS;
+}
+
+static int validate_recv_data_frame(struct adapter *adapter,
+ struct recv_frame *precv_frame)
+{
+ u8 bretry;
+ u8 *psa, *pda, *pbssid;
+ struct sta_info *psta = NULL;
+ u8 *ptr = precv_frame->rx_data;
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ struct security_priv *psecuritypriv = &adapter->securitypriv;
+ int ret = _SUCCESS;
+
+ bretry = GetRetry(ptr);
+ pda = get_da(ptr);
+ psa = get_sa(ptr);
+ pbssid = get_hdr_bssid(ptr);
+
+ if (pbssid == NULL) {
+ ret = _FAIL;
+ goto exit;
+ }
+
+ memcpy(pattrib->dst, pda, ETH_ALEN);
+ memcpy(pattrib->src, psa, ETH_ALEN);
+
+ memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+ switch (pattrib->to_fr_ds) {
+ case 0:
+ memcpy(pattrib->ra, pda, ETH_ALEN);
+ memcpy(pattrib->ta, psa, ETH_ALEN);
+ ret = sta2sta_data_frame(adapter, precv_frame, &psta);
+ break;
+ case 1:
+ memcpy(pattrib->ra, pda, ETH_ALEN);
+ memcpy(pattrib->ta, pbssid, ETH_ALEN);
+ ret = ap2sta_data_frame(adapter, precv_frame, &psta);
+ break;
+ case 2:
+ memcpy(pattrib->ra, pbssid, ETH_ALEN);
+ memcpy(pattrib->ta, psa, ETH_ALEN);
+ ret = sta2ap_data_frame(adapter, precv_frame, &psta);
+ break;
+ case 3:
+ memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+ memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+ ret = _FAIL;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
+ break;
+ default:
+ ret = _FAIL;
+ break;
+ }
+
+ if (ret == _FAIL) {
+ goto exit;
+ } else if (ret == RTW_RX_HANDLED) {
+ goto exit;
+ }
+
+ if (psta == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" after to_fr_ds_chk; psta==NULL\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ /* psta->rssi = prxcmd->rssi; */
+ /* psta->signal_quality = prxcmd->sq; */
+ precv_frame->psta = psta;
+
+ pattrib->amsdu = 0;
+ pattrib->ack_policy = 0;
+ /* parsing QC field */
+ if (pattrib->qos == 1) {
+ pattrib->priority = GetPriority((ptr + 24));
+ pattrib->ack_policy = GetAckpolicy((ptr + 24));
+ pattrib->amsdu = GetAMsdu((ptr + 24));
+ pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26;
+
+ if (pattrib->priority != 0 && pattrib->priority != 3)
+ adapter->recvpriv.bIsAnyNonBEPkts = true;
+ } else {
+ pattrib->priority = 0;
+ pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24;
+ }
+
+ if (pattrib->order)/* HT-CTRL 11n */
+ pattrib->hdrlen += 4;
+
+ precv_frame->preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+ /* decache, drop duplicate recv packets */
+ if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) == _FAIL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decache : drop pkt\n"));
+ ret = _FAIL;
+ goto exit;
+ }
+
+ if (pattrib->privacy) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("validate_recv_data_frame:pattrib->privacy=%x\n", pattrib->privacy));
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x))=%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0], IS_MCAST(pattrib->ra)));
+
+ GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n pattrib->encrypt=%d\n", pattrib->encrypt));
+
+ SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+ } else {
+ pattrib->encrypt = 0;
+ pattrib->iv_len = 0;
+ pattrib->icv_len = 0;
+ }
+
+exit:
+
+ return ret;
+}
+
+static int validate_recv_frame(struct adapter *adapter, struct recv_frame *precv_frame)
+{
+ /* shall check frame subtype, to / from ds, da, bssid */
+
+ /* then call check if rx seq/frag. duplicated. */
+
+ u8 type;
+ u8 subtype;
+ int retval = _SUCCESS;
+ u8 bDumpRxPkt;
+ struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
+ u8 *ptr = precv_frame->rx_data;
+ u8 ver = (unsigned char) (*ptr)&0x3;
+ struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+
+ if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+ int ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, rtw_get_oper_ch(adapter));
+ if (ch_set_idx >= 0)
+ pmlmeext->channel_set[ch_set_idx].rx_count++;
+ }
+
+ /* add version chk */
+ if (ver != 0) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! (ver!=0)\n"));
+ retval = _FAIL;
+ goto exit;
+ }
+
+ type = GetFrameType(ptr);
+ subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */
+
+ pattrib->to_fr_ds = get_tofr_ds(ptr);
+
+ pattrib->frag_num = GetFragNum(ptr);
+ pattrib->seq_num = GetSequence(ptr);
+
+ pattrib->pw_save = GetPwrMgt(ptr);
+ pattrib->mfrag = GetMFrag(ptr);
+ pattrib->mdata = GetMData(ptr);
+ pattrib->privacy = GetPrivacy(ptr);
+ pattrib->order = GetOrder(ptr);
+
+ /* Dump rx packets */
+ rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+ if (bDumpRxPkt == 1) {/* dump all rx packets */
+ int i;
+ DBG_88E("#############################\n");
+
+ for (i = 0; i < 64; i = i+8)
+ DBG_88E("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+ *(ptr+i+1), *(ptr+i+2), *(ptr+i+3), *(ptr+i+4), *(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+ DBG_88E("#############################\n");
+ } else if (bDumpRxPkt == 2) {
+ if (type == WIFI_MGT_TYPE) {
+ int i;
+ DBG_88E("#############################\n");
+
+ for (i = 0; i < 64; i = i+8)
+ DBG_88E("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+ *(ptr+i+1), *(ptr+i+2), *(ptr+i+3), *(ptr+i+4), *(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+ DBG_88E("#############################\n");
+ }
+ } else if (bDumpRxPkt == 3) {
+ if (type == WIFI_DATA_TYPE) {
+ int i;
+ DBG_88E("#############################\n");
+
+ for (i = 0; i < 64; i = i+8)
+ DBG_88E("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+ *(ptr+i+1), *(ptr+i+2), *(ptr+i+3), *(ptr+i+4), *(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+ DBG_88E("#############################\n");
+ }
+ }
+ switch (type) {
+ case WIFI_MGT_TYPE: /* mgnt */
+ retval = validate_recv_mgnt_frame(adapter, precv_frame);
+ if (retval == _FAIL)
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_mgnt_frame fail\n"));
+ retval = _FAIL; /* only data frame return _SUCCESS */
+ break;
+ case WIFI_CTRL_TYPE: /* ctrl */
+ retval = validate_recv_ctrl_frame(adapter, precv_frame);
+ if (retval == _FAIL)
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_ctrl_frame fail\n"));
+ retval = _FAIL; /* only data frame return _SUCCESS */
+ break;
+ case WIFI_DATA_TYPE: /* data */
+ rtw_led_control(adapter, LED_CTL_RX);
+ pattrib->qos = (subtype & BIT(7)) ? 1 : 0;
+ retval = validate_recv_data_frame(adapter, precv_frame);
+ if (retval == _FAIL) {
+ struct recv_priv *precvpriv = &adapter->recvpriv;
+ precvpriv->rx_drop++;
+ }
+ break;
+ default:
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! type= 0x%x\n", type));
+ retval = _FAIL;
+ break;
+ }
+
+exit:
+
+ return retval;
+}
+
+/* remove the wlanhdr and add the eth_hdr */
+
+static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
+{
+ int rmv_len;
+ u16 eth_type, len;
+ __be16 be_tmp;
+ u8 bsnaphdr;
+ u8 *psnap_type;
+ struct ieee80211_snap_hdr *psnap;
+
+ int ret = _SUCCESS;
+ struct adapter *adapter = precvframe->adapter;
+ struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+ u8 *ptr = get_recvframe_data(precvframe); /* point to frame_ctrl field */
+ struct rx_pkt_attrib *pattrib = &precvframe->attrib;
+
+ if (pattrib->encrypt)
+ recvframe_pull_tail(precvframe, pattrib->icv_len);
+
+ psnap = (struct ieee80211_snap_hdr *)(ptr+pattrib->hdrlen + pattrib->iv_len);
+ psnap_type = ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+ /* convert hdr + possible LLC headers into Ethernet header */
+ if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
+ memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) &&
+ memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2)) ||
+ !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) {
+ /* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+ bsnaphdr = true;
+ } else {
+ /* Leave Ethernet header part of hdr and full payload */
+ bsnaphdr = false;
+ }
+
+ rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr ? SNAP_SIZE : 0);
+ len = precvframe->len - rmv_len;
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+ ("\n===pattrib->hdrlen: %x, pattrib->iv_len:%x===\n\n", pattrib->hdrlen, pattrib->iv_len));
+
+ memcpy(&be_tmp, ptr+rmv_len, 2);
+ eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+ pattrib->eth_type = eth_type;
+
+ if ((check_fwstate(pmlmepriv, WIFI_MP_STATE))) {
+ ptr += rmv_len;
+ *ptr = 0x87;
+ *(ptr+1) = 0x12;
+
+ eth_type = 0x8712;
+ /* append rx status for mp test packets */
+ ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24);
+ memcpy(ptr, get_rxmem(precvframe), 24);
+ ptr += 24;
+ } else {
+ ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr) + (bsnaphdr ? 2 : 0)));
+ }
+
+ memcpy(ptr, pattrib->dst, ETH_ALEN);
+ memcpy(ptr+ETH_ALEN, pattrib->src, ETH_ALEN);
+
+ if (!bsnaphdr) {
+ be_tmp = htons(len);
+ memcpy(ptr+12, &be_tmp, 2);
+ }
+
+ return ret;
+}
+
+/* perform defrag */
+static struct recv_frame *recvframe_defrag(struct adapter *adapter, struct __queue *defrag_q)
+{
+ struct list_head *plist, *phead;
+ u8 wlanhdr_offset;
+ u8 curfragnum;
+ struct recv_frame *pfhdr, *pnfhdr;
+ struct recv_frame *prframe, *pnextrframe;
+ struct __queue *pfree_recv_queue;
+
+ curfragnum = 0;
+ pfree_recv_queue = &adapter->recvpriv.free_recv_queue;
+
+ phead = get_list_head(defrag_q);
+ plist = phead->next;
+ pfhdr = container_of(plist, struct recv_frame, list);
+ prframe = (struct recv_frame *)pfhdr;
+ list_del_init(&(prframe->list));
+
+ if (curfragnum != pfhdr->attrib.frag_num) {
+ /* the first fragment number must be 0 */
+ /* free the whole queue */
+ rtw_free_recvframe(prframe, pfree_recv_queue);
+ rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+ return NULL;
+ }
+
+ curfragnum++;
+
+ plist = get_list_head(defrag_q);
+ plist = phead->next;
+ pfhdr = container_of(plist, struct recv_frame, list);
+ prframe = (struct recv_frame *)pfhdr;
+ list_del_init(&(prframe->list));
+
+ plist = plist->next;
+
+ while (phead != plist) {
+ pnfhdr = container_of(plist, struct recv_frame, list);
+ pnextrframe = (struct recv_frame *)pnfhdr;
+
+ /* check the fragment sequence (2nd ~n fragment frame) */
+
+ if (curfragnum != pnfhdr->attrib.frag_num) {
+ /* the fragment number must be increasing (after decache) */
+ /* release the defrag_q & prframe */
+ rtw_free_recvframe(prframe, pfree_recv_queue);
+ rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+ return NULL;
+ }
+
+ curfragnum++;
+
+ /* copy the 2nd~n fragment frame's payload to the first fragment */
+ /* get the 2nd~last fragment frame's payload */
+
+ wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+
+ recvframe_pull(pnextrframe, wlanhdr_offset);
+
+ /* append to first fragment frame's tail (if privacy frame, pull the ICV) */
+ recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+
+ /* memcpy */
+ memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+
+ recvframe_put(prframe, pnfhdr->len);
+
+ pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+ plist = plist->next;
+ }
+
+ /* free the defrag_q queue and return the prframe */
+ rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Performance defrag!!!!!\n"));
+
+ return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+struct recv_frame *recvframe_chk_defrag(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+ u8 ismfrag;
+ u8 fragnum;
+ u8 *psta_addr;
+ struct recv_frame *pfhdr;
+ struct sta_info *psta;
+ struct sta_priv *pstapriv;
+ struct list_head *phead;
+ struct recv_frame *prtnframe = NULL;
+ struct __queue *pfree_recv_queue, *pdefrag_q;
+
+ pstapriv = &padapter->stapriv;
+
+ pfhdr = precv_frame;
+
+ pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+ /* need to define struct of wlan header frame ctrl */
+ ismfrag = pfhdr->attrib.mfrag;
+ fragnum = pfhdr->attrib.frag_num;
+
+ psta_addr = pfhdr->attrib.ta;
+ psta = rtw_get_stainfo(pstapriv, psta_addr);
+ if (psta == NULL) {
+ u8 type = GetFrameType(pfhdr->rx_data);
+ if (type != WIFI_DATA_TYPE) {
+ psta = rtw_get_bcmc_stainfo(padapter);
+ pdefrag_q = &psta->sta_recvpriv.defrag_q;
+ } else {
+ pdefrag_q = NULL;
+ }
+ } else {
+ pdefrag_q = &psta->sta_recvpriv.defrag_q;
+ }
+
+ if ((ismfrag == 0) && (fragnum == 0))
+ prtnframe = precv_frame;/* isn't a fragment frame */
+
+ if (ismfrag == 1) {
+ /* 0~(n-1) fragment frame */
+ /* enqueue to defraf_g */
+ if (pdefrag_q != NULL) {
+ if (fragnum == 0) {
+ /* the first fragment */
+ if (!list_empty(&pdefrag_q->queue)) {
+ /* free current defrag_q */
+ rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue);
+ }
+ }
+
+ /* Then enqueue the 0~(n-1) fragment into the defrag_q */
+
+ phead = get_list_head(pdefrag_q);
+ list_add_tail(&pfhdr->list, phead);
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Enqueuq: ismfrag=%d, fragnum=%d\n", ismfrag, fragnum));
+
+ prtnframe = NULL;
+ } else {
+ /* can't find this ta's defrag_queue, so free this recv_frame */
+ if (precv_frame && pfree_recv_queue)
+ rtw_free_recvframe(precv_frame, pfree_recv_queue);
+ prtnframe = NULL;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q==NULL: ismfrag=%d, fragnum=%d\n", ismfrag, fragnum));
+ }
+ }
+
+ if ((ismfrag == 0) && (fragnum != 0)) {
+ /* the last fragment frame */
+ /* enqueue the last fragment */
+ if (pdefrag_q != NULL) {
+ phead = get_list_head(pdefrag_q);
+ list_add_tail(&pfhdr->list, phead);
+
+ /* call recvframe_defrag to defrag */
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("defrag: ismfrag=%d, fragnum=%d\n", ismfrag, fragnum));
+ precv_frame = recvframe_defrag(padapter, pdefrag_q);
+ prtnframe = precv_frame;
+ } else {
+ /* can't find this ta's defrag_queue, so free this recv_frame */
+ if (precv_frame && pfree_recv_queue)
+ rtw_free_recvframe(precv_frame, pfree_recv_queue);
+ prtnframe = NULL;
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q==NULL: ismfrag=%d, fragnum=%d\n", ismfrag, fragnum));
+ }
+ }
+
+ if ((prtnframe != NULL) && (prtnframe->attrib.privacy)) {
+ /* after defrag we must check tkip mic code */
+ if (recvframe_chkmic(padapter, prtnframe) == _FAIL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic(padapter, prtnframe)==_FAIL\n"));
+ if (precv_frame && pfree_recv_queue)
+ rtw_free_recvframe(prtnframe, pfree_recv_queue);
+ prtnframe = NULL;
+ }
+ }
+
+ return prtnframe;
+}
+
+static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
+{
+ int a_len, padding_len;
+ u16 eth_type, nSubframe_Length;
+ u8 nr_subframes, i;
+ unsigned char *pdata;
+ struct rx_pkt_attrib *pattrib;
+ unsigned char *data_ptr;
+ struct sk_buff *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+ struct __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
+ int ret = _SUCCESS;
+ nr_subframes = 0;
+
+ pattrib = &prframe->attrib;
+
+ recvframe_pull(prframe, prframe->attrib.hdrlen);
+
+ if (prframe->attrib.iv_len > 0)
+ recvframe_pull(prframe, prframe->attrib.iv_len);
+
+ a_len = prframe->len;
+
+ pdata = prframe->rx_data;
+
+ while (a_len > ETH_HLEN) {
+ /* Offset 12 denote 2 mac address */
+ nSubframe_Length = RTW_GET_BE16(pdata + 12);
+
+ if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+ DBG_88E("nRemain_Length is %d and nSubframe_Length is : %d\n", a_len, nSubframe_Length);
+ goto exit;
+ }
+
+ /* move the data point to data content */
+ pdata += ETH_HLEN;
+ a_len -= ETH_HLEN;
+
+ /* Allocate new skb for releasing to upper layer */
+ sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+ if (sub_skb) {
+ skb_reserve(sub_skb, 12);
+ data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
+ memcpy(data_ptr, pdata, nSubframe_Length);
+ } else {
+ sub_skb = skb_clone(prframe->pkt, GFP_ATOMIC);
+ if (sub_skb) {
+ sub_skb->data = pdata;
+ sub_skb->len = nSubframe_Length;
+ skb_set_tail_pointer(sub_skb, nSubframe_Length);
+ } else {
+ DBG_88E("skb_clone() Fail!!! , nr_subframes=%d\n", nr_subframes);
+ break;
+ }
+ }
+
+ subframes[nr_subframes++] = sub_skb;
+
+ if (nr_subframes >= MAX_SUBFRAME_COUNT) {
+ DBG_88E("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+ break;
+ }
+
+ pdata += nSubframe_Length;
+ a_len -= nSubframe_Length;
+ if (a_len != 0) {
+ padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4-1));
+ if (padding_len == 4) {
+ padding_len = 0;
+ }
+
+ if (a_len < padding_len) {
+ goto exit;
+ }
+ pdata += padding_len;
+ a_len -= padding_len;
+ }
+ }
+
+ for (i = 0; i < nr_subframes; i++) {
+ sub_skb = subframes[i];
+ /* convert hdr + possible LLC headers into Ethernet header */
+ eth_type = RTW_GET_BE16(&sub_skb->data[6]);
+ if (sub_skb->len >= 8 &&
+ ((!memcmp(sub_skb->data, rtw_rfc1042_header, SNAP_SIZE) &&
+ eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+ !memcmp(sub_skb->data, rtw_bridge_tunnel_header, SNAP_SIZE))) {
+ /* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+ skb_pull(sub_skb, SNAP_SIZE);
+ memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+ memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+ } else {
+ __be16 len;
+ /* Leave Ethernet header part of hdr and full payload */
+ len = htons(sub_skb->len);
+ memcpy(skb_push(sub_skb, 2), &len, 2);
+ memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+ memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+ }
+
+ /* Indicate the packets to upper layer */
+ /* Insert NAT2.5 RX here! */
+ sub_skb->protocol = eth_type_trans(sub_skb, padapter->pnetdev);
+ sub_skb->dev = padapter->pnetdev;
+
+ sub_skb->ip_summed = CHECKSUM_NONE;
+
+ netif_rx(sub_skb);
+ }
+
+exit:
+
+ prframe->len = 0;
+ rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */
+
+ return ret;
+}
+
+static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
+{
+ u8 wsize = preorder_ctrl->wsize_b;
+ u16 wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF;/* 4096; */
+
+ /* Rx Reorder initialize condition. */
+ if (preorder_ctrl->indicate_seq == 0xFFFF)
+ preorder_ctrl->indicate_seq = seq_num;
+
+ /* Drop out the packet which SeqNum is smaller than WinStart */
+ if (SN_LESS(seq_num, preorder_ctrl->indicate_seq))
+ return false;
+
+ /* */
+ /* Sliding window manipulation. Conditions includes: */
+ /* 1. Incoming SeqNum is equal to WinStart =>Window shift 1 */
+ /* 2. Incoming SeqNum is larger than the WinEnd => Window shift N */
+ /* */
+ if (SN_EQUAL(seq_num, preorder_ctrl->indicate_seq)) {
+ preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+ } else if (SN_LESS(wend, seq_num)) {
+ if (seq_num >= (wsize - 1))
+ preorder_ctrl->indicate_seq = seq_num + 1 - wsize;
+ else
+ preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1;
+ }
+
+ return true;
+}
+
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, struct recv_frame *prframe);
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, struct recv_frame *prframe)
+{
+ struct rx_pkt_attrib *pattrib = &prframe->attrib;
+ struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+ struct list_head *phead, *plist;
+ struct recv_frame *hdr;
+ struct rx_pkt_attrib *pnextattrib;
+
+ phead = get_list_head(ppending_recvframe_queue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ hdr = container_of(plist, struct recv_frame, list);
+ pnextattrib = &hdr->attrib;
+
+ if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+ plist = plist->next;
+ else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+ return false;
+ else
+ break;
+ }
+
+ list_del_init(&(prframe->list));
+
+ list_add_tail(&(prframe->list), plist);
+ return true;
+}
+
+static int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+ struct list_head *phead, *plist;
+ struct recv_frame *prframe;
+ struct rx_pkt_attrib *pattrib;
+ int bPktInBuf = false;
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+ struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+ phead = get_list_head(ppending_recvframe_queue);
+ plist = phead->next;
+
+ /* Handling some condition for forced indicate case. */
+ if (bforced) {
+ if (list_empty(phead))
+ return true;
+
+ prframe = container_of(plist, struct recv_frame, list);
+ pattrib = &prframe->attrib;
+ preorder_ctrl->indicate_seq = pattrib->seq_num;
+ }
+
+ /* Prepare indication list and indication. */
+ /* Check if there is any packet need indicate. */
+ while (!list_empty(phead)) {
+ prframe = container_of(plist, struct recv_frame, list);
+ pattrib = &prframe->attrib;
+
+ if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+ ("recv_indicatepkts_in_order: indicate=%d seq=%d amsdu=%d\n",
+ preorder_ctrl->indicate_seq, pattrib->seq_num, pattrib->amsdu));
+ plist = plist->next;
+ list_del_init(&(prframe->list));
+
+ if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num))
+ preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+ /* Set this as a lock to make sure that only one thread is indicating packet. */
+
+ /* indicate this recv_frame */
+ if (!pattrib->amsdu) {
+ if ((!padapter->bDriverStopped) &&
+ (!padapter->bSurpriseRemoved))
+ rtw_recv_indicatepkt(padapter, prframe);/* indicate this recv_frame */
+ } else if (pattrib->amsdu == 1) {
+ if (amsdu_to_msdu(padapter, prframe) != _SUCCESS)
+ rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+ } else {
+ /* error condition; */
+ }
+
+ /* Update local variables. */
+ bPktInBuf = false;
+ } else {
+ bPktInBuf = true;
+ break;
+ }
+ }
+ return bPktInBuf;
+}
+
+static int recv_indicatepkt_reorder(struct adapter *padapter, struct recv_frame *prframe)
+{
+ int retval = _SUCCESS;
+ struct rx_pkt_attrib *pattrib = &prframe->attrib;
+ struct recv_reorder_ctrl *preorder_ctrl = prframe->preorder_ctrl;
+ struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+ if (!pattrib->amsdu) {
+ /* s1. */
+ wlanhdr_to_ethhdr(prframe);
+
+ if (pattrib->qos != 1) {
+ if (!padapter->bDriverStopped &&
+ !padapter->bSurpriseRemoved) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+ ("@@@@ recv_indicatepkt_reorder -recv_func recv_indicatepkt\n"));
+
+ rtw_recv_indicatepkt(padapter, prframe);
+ return _SUCCESS;
+ }
+
+ return _FAIL;
+ }
+
+ if (!preorder_ctrl->enable) {
+ /* indicate this recv_frame */
+ preorder_ctrl->indicate_seq = pattrib->seq_num;
+ rtw_recv_indicatepkt(padapter, prframe);
+
+ preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+ return _SUCCESS;
+ }
+ } else if (pattrib->amsdu == 1) { /* temp filter -> means didn't support A-MSDUs in a A-MPDU */
+ if (!preorder_ctrl->enable) {
+ preorder_ctrl->indicate_seq = pattrib->seq_num;
+ retval = amsdu_to_msdu(padapter, prframe);
+
+ preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+ return retval;
+ }
+ }
+
+ spin_lock_bh(&ppending_recvframe_queue->lock);
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+ ("recv_indicatepkt_reorder: indicate=%d seq=%d\n",
+ preorder_ctrl->indicate_seq, pattrib->seq_num));
+
+ /* s2. check if winstart_b(indicate_seq) needs to been updated */
+ if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num))
+ goto _err_exit;
+
+ /* s3. Insert all packet into Reorder Queue to maintain its ordering. */
+ if (!enqueue_reorder_recvframe(preorder_ctrl, prframe))
+ goto _err_exit;
+
+ /* s4. */
+ /* Indication process. */
+ /* After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */
+ /* with the SeqNum smaller than latest WinStart and buffer other packets. */
+ /* */
+ /* For Rx Reorder condition: */
+ /* 1. All packets with SeqNum smaller than WinStart => Indicate */
+ /* 2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */
+ /* */
+
+ /* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */
+ if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false)) {
+ _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+ spin_unlock_bh(&ppending_recvframe_queue->lock);
+ } else {
+ spin_unlock_bh(&ppending_recvframe_queue->lock);
+ _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+ }
+
+_success_exit:
+
+ return _SUCCESS;
+
+_err_exit:
+
+ spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+ return _FAIL;
+}
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+{
+ struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+ struct adapter *padapter = preorder_ctrl->padapter;
+ struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+ if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+ return;
+
+ spin_lock_bh(&ppending_recvframe_queue->lock);
+
+ if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true)
+ _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+
+ spin_unlock_bh(&ppending_recvframe_queue->lock);
+}
+
+static int process_recv_indicatepkts(struct adapter *padapter, struct recv_frame *prframe)
+{
+ int retval = _SUCCESS;
+ /* struct recv_priv *precvpriv = &padapter->recvpriv; */
+ /* struct rx_pkt_attrib *pattrib = &prframe->attrib; */
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+ if (phtpriv->ht_option) { /* B/G/N Mode */
+ /* prframe->preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */
+
+ if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) {
+ /* including perform A-MPDU Rx Ordering Buffer Control */
+ if ((!padapter->bDriverStopped) &&
+ (!padapter->bSurpriseRemoved)) {
+ retval = _FAIL;
+ return retval;
+ }
+ }
+ } else { /* B/G mode */
+ retval = wlanhdr_to_ethhdr (prframe);
+ if (retval != _SUCCESS) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("wlanhdr_to_ethhdr: drop pkt\n"));
+ return retval;
+ }
+
+ if ((!padapter->bDriverStopped) &&
+ (!padapter->bSurpriseRemoved)) {
+ /* indicate this recv_frame */
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func recv_indicatepkt\n"));
+ rtw_recv_indicatepkt(padapter, prframe);
+ } else {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n"));
+
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+ retval = _FAIL;
+ return retval;
+ }
+ }
+
+ return retval;
+}
+
+static int recv_func_prehandle(struct adapter *padapter, struct recv_frame *rframe)
+{
+ int ret = _SUCCESS;
+ struct rx_pkt_attrib *pattrib = &rframe->attrib;
+ struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if (padapter->registrypriv.mp_mode == 1) {
+ if (pattrib->crc_err == 1)
+ padapter->mppriv.rx_crcerrpktcount++;
+ else
+ padapter->mppriv.rx_pktcount++;
+
+ if (check_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE) == false) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_alert_, ("MP - Not in loopback mode , drop pkt\n"));
+ ret = _FAIL;
+ rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+ goto exit;
+ }
+ }
+
+ /* check the frame crtl field and decache */
+ ret = validate_recv_frame(padapter, rframe);
+ if (ret != _SUCCESS) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
+ rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+ goto exit;
+ }
+
+exit:
+ return ret;
+}
+
+static int recv_func_posthandle(struct adapter *padapter, struct recv_frame *prframe)
+{
+ int ret = _SUCCESS;
+ struct recv_frame *orig_prframe = prframe;
+ struct recv_priv *precvpriv = &padapter->recvpriv;
+ struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+ /* DATA FRAME */
+ rtw_led_control(padapter, LED_CTL_RX);
+
+ prframe = decryptor(padapter, prframe);
+ if (prframe == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decryptor: drop pkt\n"));
+ ret = _FAIL;
+ goto _recv_data_drop;
+ }
+
+ prframe = recvframe_chk_defrag(padapter, prframe);
+ if (prframe == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chk_defrag: drop pkt\n"));
+ goto _recv_data_drop;
+ }
+
+ prframe = portctrl(padapter, prframe);
+ if (prframe == NULL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("portctrl: drop pkt\n"));
+ ret = _FAIL;
+ goto _recv_data_drop;
+ }
+
+ count_rx_stats(padapter, prframe, NULL);
+
+ ret = process_recv_indicatepkts(padapter, prframe);
+ if (ret != _SUCCESS) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recv_func: process_recv_indicatepkts fail!\n"));
+ rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+ goto _recv_data_drop;
+ }
+ return ret;
+
+_recv_data_drop:
+ precvpriv->rx_drop++;
+ return ret;
+}
+
+static int recv_func(struct adapter *padapter, struct recv_frame *rframe)
+{
+ int ret;
+ struct rx_pkt_attrib *prxattrib = &rframe->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+ struct recv_priv *recvpriv = &padapter->recvpriv;
+
+ /* check if need to handle uc_swdec_pending_queue*/
+ if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+ psecuritypriv->busetkipkey) {
+ struct recv_frame *pending_frame;
+ int cnt = 0;
+
+ pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue);
+ while (pending_frame) {
+ cnt++;
+ recv_func_posthandle(padapter, pending_frame);
+ }
+ }
+
+ ret = recv_func_prehandle(padapter, rframe);
+
+ if (ret == _SUCCESS) {
+ /* check if need to enqueue into uc_swdec_pending_queue*/
+ if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+ !IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 &&
+ (prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt) &&
+ psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK &&
+ !psecuritypriv->busetkipkey) {
+ rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+ DBG_88E("%s: no key, enqueue uc_swdec_pending_queue\n", __func__);
+ if (recvpriv->free_recvframe_cnt < NR_RECVFRAME/4) {
+ /* to prevent from recvframe starvation,
+ * get recvframe from uc_swdec_pending_queue to
+ * free_recvframe_cnt */
+ rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue);
+ if (rframe)
+ goto do_posthandle;
+ }
+ goto exit;
+ }
+do_posthandle:
+ ret = recv_func_posthandle(padapter, rframe);
+ }
+
+exit:
+ return ret;
+}
+
+s32 rtw_recv_entry(struct recv_frame *precvframe)
+{
+ struct adapter *padapter;
+ struct recv_priv *precvpriv;
+ s32 ret = _SUCCESS;
+
+ padapter = precvframe->adapter;
+
+ precvpriv = &padapter->recvpriv;
+
+ ret = recv_func(padapter, precvframe);
+ if (ret == _FAIL) {
+ RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("rtw_recv_entry: recv_func return fail!!!\n"));
+ goto _recv_entry_drop;
+ }
+
+ precvpriv->rx_pkts++;
+
+ return ret;
+
+_recv_entry_drop:
+
+ if (padapter->registrypriv.mp_mode == 1)
+ padapter->mppriv.rx_pktloss = precvpriv->rx_drop;
+
+ return ret;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+void rtw_signal_stat_timer_hdl(struct timer_list *t)
+#else
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+ struct adapter *adapter = from_timer(adapter, t, recvpriv.signal_stat_timer);
+#else
+ struct adapter *adapter = (struct adapter *)FunctionContext;
+#endif
+ struct recv_priv *recvpriv = &adapter->recvpriv;
+
+ u32 tmp_s, tmp_q;
+ u8 avg_signal_strength = 0;
+ u8 avg_signal_qual = 0;
+ u8 _alpha = 3; /* this value is based on converging_constant = 5000 and sampling_interval = 1000 */
+
+ if (adapter->recvpriv.is_signal_dbg) {
+ /* update the user specific value, signal_strength_dbg, to signal_strength, rssi */
+ adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg;
+ adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+ } else {
+ if (recvpriv->signal_strength_data.update_req == 0) {/* update_req is clear, means we got rx */
+ avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+ /* after avg_vals are accquired, we can re-stat the signal values */
+ recvpriv->signal_strength_data.update_req = 1;
+ }
+
+ if (recvpriv->signal_qual_data.update_req == 0) {/* update_req is clear, means we got rx */
+ avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+ /* after avg_vals are accquired, we can re-stat the signal values */
+ recvpriv->signal_qual_data.update_req = 1;
+ }
+
+ /* update value of signal_strength, rssi, signal_qual */
+ if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == false) {
+ tmp_s = (avg_signal_strength+(_alpha-1)*recvpriv->signal_strength);
+ if (tmp_s % _alpha)
+ tmp_s = tmp_s/_alpha + 1;
+ else
+ tmp_s = tmp_s/_alpha;
+ if (tmp_s > 100)
+ tmp_s = 100;
+
+ tmp_q = (avg_signal_qual+(_alpha-1)*recvpriv->signal_qual);
+ if (tmp_q % _alpha)
+ tmp_q = tmp_q/_alpha + 1;
+ else
+ tmp_q = tmp_q/_alpha;
+ if (tmp_q > 100)
+ tmp_q = 100;
+
+ recvpriv->signal_strength = tmp_s;
+ recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+ recvpriv->signal_qual = tmp_q;
+ }
+ }
+ rtw_set_signal_stat_timer(recvpriv);
+}
diff --git a/drivers/staging/r8188eu/core/rtw_rf.c b/drivers/staging/r8188eu/core/rtw_rf.c
new file mode 100644
index 000000000000..40c0f5aa0731
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_rf.c
@@ -0,0 +1,88 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_RF_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+
+struct ch_freq {
+ u32 channel;
+ u32 frequency;
+};
+
+static struct ch_freq ch_freq_map[] = {
+ {1, 2412}, {2, 2417}, {3, 2422}, {4, 2427}, {5, 2432},
+ {6, 2437}, {7, 2442}, {8, 2447}, {9, 2452}, {10, 2457},
+ {11, 2462}, {12, 2467}, {13, 2472}, {14, 2484},
+ /* UNII */
+ {36, 5180}, {40, 5200}, {44, 5220}, {48, 5240}, {52, 5260},
+ {56, 5280}, {60, 5300}, {64, 5320}, {149, 5745}, {153, 5765},
+ {157, 5785}, {161, 5805}, {165, 5825}, {167, 5835}, {169, 5845},
+ {171, 5855}, {173, 5865},
+ /* HiperLAN2 */
+ {100, 5500}, {104, 5520}, {108, 5540}, {112, 5560}, {116, 5580},
+ {120, 5600}, {124, 5620}, {128, 5640}, {132, 5660}, {136, 5680},
+ {140, 5700},
+ /* Japan MMAC */
+ {34, 5170}, {38, 5190}, {42, 5210}, {46, 5230},
+ /* Japan */
+ {184, 4920}, {188, 4940}, {192, 4960}, {196, 4980},
+ {208, 5040},/* Japan, means J08 */
+ {212, 5060},/* Japan, means J12 */
+ {216, 5080},/* Japan, means J16 */
+};
+
+static int ch_freq_map_num = (sizeof(ch_freq_map) / sizeof(struct ch_freq));
+
+u32 rtw_ch2freq(u32 channel)
+{
+ u8 i;
+ u32 freq = 0;
+
+ for (i = 0; i < ch_freq_map_num; i++) {
+ if (channel == ch_freq_map[i].channel) {
+ freq = ch_freq_map[i].frequency;
+ break;
+ }
+ }
+ if (i == ch_freq_map_num)
+ freq = 2412;
+
+ return freq;
+}
+
+u32 rtw_freq2ch(u32 freq)
+{
+ u8 i;
+ u32 ch = 0;
+
+ for (i = 0; i < ch_freq_map_num; i++) {
+ if (freq == ch_freq_map[i].frequency) {
+ ch = ch_freq_map[i].channel;
+ break;
+ }
+ }
+ if (i == ch_freq_map_num)
+ ch = 1;
+
+ return ch;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_security.c b/drivers/staging/r8188eu/core/rtw_security.c
new file mode 100644
index 000000000000..1da908694e18
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_security.c
@@ -0,0 +1,1757 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_SECURITY_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <osdep_intf.h>
+
+/* WEP related ===== */
+
+#define CRC32_POLY 0x04c11db7
+
+struct arc4context {
+ u32 x;
+ u32 y;
+ u8 state[256];
+};
+
+static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
+{
+ u32 t, u;
+ u32 keyindex;
+ u32 stateindex;
+ u8 *state;
+ u32 counter;
+
+ state = parc4ctx->state;
+ parc4ctx->x = 0;
+ parc4ctx->y = 0;
+ for (counter = 0; counter < 256; counter++)
+ state[counter] = (u8)counter;
+ keyindex = 0;
+ stateindex = 0;
+ for (counter = 0; counter < 256; counter++) {
+ t = state[counter];
+ stateindex = (stateindex + key[keyindex] + t) & 0xff;
+ u = state[stateindex];
+ state[stateindex] = (u8)t;
+ state[counter] = (u8)u;
+ if (++keyindex >= key_len)
+ keyindex = 0;
+ }
+
+}
+
+static u32 arcfour_byte(struct arc4context *parc4ctx)
+{
+ u32 x;
+ u32 y;
+ u32 sx, sy;
+ u8 *state;
+
+ state = parc4ctx->state;
+ x = (parc4ctx->x + 1) & 0xff;
+ sx = state[x];
+ y = (sx + parc4ctx->y) & 0xff;
+ sy = state[y];
+ parc4ctx->x = x;
+ parc4ctx->y = y;
+ state[y] = (u8)sx;
+ state[x] = (u8)sy;
+
+ return state[(sx + sy) & 0xff];
+}
+
+static void arcfour_encrypt(struct arc4context *parc4ctx, u8 *dest, u8 *src, u32 len)
+{
+ u32 i;
+
+ for (i = 0; i < len; i++)
+ dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
+
+}
+
+static int bcrc32initialized;
+static u32 crc32_table[256];
+
+static u8 crc32_reverseBit(u8 data)
+{
+ return (u8)((data<<7)&0x80) | ((data<<5)&0x40) | ((data<<3)&0x20) |
+ ((data<<1)&0x10) | ((data>>1)&0x08) | ((data>>3)&0x04) |
+ ((data>>5)&0x02) | ((data>>7)&0x01);
+}
+
+static void crc32_init(void)
+{
+ if (bcrc32initialized == 1) {
+ return;
+ } else {
+ int i, j;
+ u32 c;
+ u8 *p = (u8 *)&c, *p1;
+ u8 k;
+
+ c = 0x12340000;
+
+ for (i = 0; i < 256; ++i) {
+ k = crc32_reverseBit((u8)i);
+ for (c = ((u32)k) << 24, j = 8; j > 0; --j)
+ c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
+ p1 = (u8 *)&crc32_table[i];
+
+ p1[0] = crc32_reverseBit(p[3]);
+ p1[1] = crc32_reverseBit(p[2]);
+ p1[2] = crc32_reverseBit(p[1]);
+ p1[3] = crc32_reverseBit(p[0]);
+ }
+ bcrc32initialized = 1;
+ }
+}
+
+static __le32 getcrc32(u8 *buf, int len)
+{
+ u8 *p;
+ u32 crc;
+
+ if (bcrc32initialized == 0)
+ crc32_init();
+
+ crc = 0xffffffff; /* preload shift register, per CRC-32 spec */
+
+ for (p = buf; len > 0; ++p, --len)
+ crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8);
+
+ return cpu_to_le32(~crc); /* transmit complement, per CRC-32 spec */
+}
+
+/*
+ Need to consider the fragment situation
+*/
+void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{ /* exclude ICV */
+
+ unsigned char crc[4];
+ struct arc4context mycontext;
+
+ int curfragnum, length;
+ u32 keylength;
+
+ u8 *pframe, *payload, *iv; /* wepkey */
+ u8 wepkey[16];
+ u8 hw_hdr_offset = 0;
+ struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+ return;
+
+ hw_hdr_offset = TXDESC_SIZE +
+ (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+
+ pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+ /* start to encrypt each fragment */
+ if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
+ keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
+
+ for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+ iv = pframe+pattrib->hdrlen;
+ memcpy(&wepkey[0], iv, 3);
+ memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
+ payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+
+ if ((curfragnum+1) == pattrib->nr_frags) { /* the last fragment */
+ length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+
+ *((__le32 *)crc) = getcrc32(payload, length);
+
+ arcfour_init(&mycontext, wepkey, 3+keylength);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+ arcfour_encrypt(&mycontext, payload+length, crc, 4);
+ } else {
+ length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+ *((__le32 *)crc) = getcrc32(payload, length);
+ arcfour_init(&mycontext, wepkey, 3+keylength);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+ arcfour_encrypt(&mycontext, payload+length, crc, 4);
+
+ pframe += pxmitpriv->frag_len;
+ pframe = (u8 *)RND4((size_t)(pframe));
+ }
+ }
+ }
+
+}
+
+void rtw_wep_decrypt(struct adapter *padapter, u8 *precvframe)
+{
+ /* exclude ICV */
+ u8 crc[4];
+ struct arc4context mycontext;
+ int length;
+ u32 keylength;
+ u8 *pframe, *payload, *iv, wepkey[16];
+ u8 keyindex;
+ struct rx_pkt_attrib *prxattrib = &(((struct recv_frame *)precvframe)->attrib);
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+ pframe = (unsigned char *)((struct recv_frame *)precvframe)->rx_data;
+
+ /* start to decrypt recvframe */
+ if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
+ iv = pframe+prxattrib->hdrlen;
+ keyindex = prxattrib->key_index;
+ keylength = psecuritypriv->dot11DefKeylen[keyindex];
+ memcpy(&wepkey[0], iv, 3);
+ memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
+ length = ((struct recv_frame *)precvframe)->len-prxattrib->hdrlen-prxattrib->iv_len;
+
+ payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+
+ /* decrypt payload include icv */
+ arcfour_init(&mycontext, wepkey, 3+keylength);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+
+ /* calculate icv and compare the icv */
+ *((__le32 *)crc) = getcrc32(payload, length - 4);
+
+ if (crc[3] != payload[length-1] ||
+ crc[2] != payload[length-2] ||
+ crc[1] != payload[length-3] ||
+ crc[0] != payload[length-4]) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+ ("rtw_wep_decrypt:icv error crc (%4ph)!=payload (%4ph)\n",
+ &crc, &payload[length-4]));
+ }
+ }
+
+ return;
+}
+
+/* 3 ===== TKIP related ===== */
+
+static u32 secmicgetuint32(u8 *p)
+/* Convert from Byte[] to Us3232 in a portable way */
+{
+ s32 i;
+ u32 res = 0;
+
+ for (i = 0; i < 4; i++)
+ res |= ((u32)(*p++)) << (8*i);
+
+ return res;
+}
+
+static void secmicputuint32(u8 *p, u32 val)
+/* Convert from Us3232 to Byte[] in a portable way */
+{
+ long i;
+
+ for (i = 0; i < 4; i++) {
+ *p++ = (u8) (val & 0xff);
+ val >>= 8;
+ }
+
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+/* Reset the state to the empty message. */
+
+ pmicdata->L = pmicdata->K0;
+ pmicdata->R = pmicdata->K1;
+ pmicdata->nBytesInM = 0;
+ pmicdata->M = 0;
+
+}
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
+{
+ /* Set the key */
+
+ pmicdata->K0 = secmicgetuint32(key);
+ pmicdata->K1 = secmicgetuint32(key + 4);
+ /* and reset the message */
+ secmicclear(pmicdata);
+
+}
+
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
+{
+
+ /* Append the byte to our word-sized buffer */
+ pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
+ pmicdata->nBytesInM++;
+ /* Process the word if it is full. */
+ if (pmicdata->nBytesInM >= 4) {
+ pmicdata->L ^= pmicdata->M;
+ pmicdata->R ^= ROL32(pmicdata->L, 17);
+ pmicdata->L += pmicdata->R;
+ pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
+ pmicdata->L += pmicdata->R;
+ pmicdata->R ^= ROL32(pmicdata->L, 3);
+ pmicdata->L += pmicdata->R;
+ pmicdata->R ^= ROR32(pmicdata->L, 2);
+ pmicdata->L += pmicdata->R;
+ /* Clear the buffer */
+ pmicdata->M = 0;
+ pmicdata->nBytesInM = 0;
+ }
+
+}
+
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
+{
+
+ /* This is simple */
+ while (nbytes > 0) {
+ rtw_secmicappendbyte(pmicdata, *src++);
+ nbytes--;
+ }
+
+}
+
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst)
+{
+
+ /* Append the minimum padding */
+ rtw_secmicappendbyte(pmicdata, 0x5a);
+ rtw_secmicappendbyte(pmicdata, 0);
+ rtw_secmicappendbyte(pmicdata, 0);
+ rtw_secmicappendbyte(pmicdata, 0);
+ rtw_secmicappendbyte(pmicdata, 0);
+ /* and then zeroes until the length is a multiple of 4 */
+ while (pmicdata->nBytesInM != 0)
+ rtw_secmicappendbyte(pmicdata, 0);
+ /* The appendByte function has already computed the result. */
+ secmicputuint32(dst, pmicdata->L);
+ secmicputuint32(dst+4, pmicdata->R);
+ /* Reset to the empty message. */
+ secmicclear(pmicdata);
+
+}
+
+void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
+{
+ struct mic_data micdata;
+ u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+
+ rtw_secmicsetkey(&micdata, key);
+ priority[0] = pri;
+
+ /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+ if (header[1]&1) { /* ToDS == 1 */
+ rtw_secmicappend(&micdata, &header[16], 6); /* DA */
+ if (header[1]&2) /* From Ds == 1 */
+ rtw_secmicappend(&micdata, &header[24], 6);
+ else
+ rtw_secmicappend(&micdata, &header[10], 6);
+ } else { /* ToDS == 0 */
+ rtw_secmicappend(&micdata, &header[4], 6); /* DA */
+ if (header[1]&2) /* From Ds == 1 */
+ rtw_secmicappend(&micdata, &header[16], 6);
+ else
+ rtw_secmicappend(&micdata, &header[10], 6);
+ }
+ rtw_secmicappend(&micdata, &priority[0], 4);
+
+ rtw_secmicappend(&micdata, data, data_len);
+
+ rtw_secgetmic(&micdata, mic_code);
+
+}
+
+/* macros for extraction/creation of unsigned char/unsigned short values */
+#define RotR1(v16) ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define Lo8(v16) ((u8)((v16) & 0x00FF))
+#define Hi8(v16) ((u8)(((v16) >> 8) & 0x00FF))
+#define Lo16(v32) ((u16)((v32) & 0xFFFF))
+#define Hi16(v32) ((u16)(((v32) >> 16) & 0xFFFF))
+#define Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[] */
+#define TK16(N) Mk16(tk[2*(N)+1], tk[2*(N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16) (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT 8 /* this needs to be "big enough" */
+#define TA_SIZE 6 /* 48-bit transmitter address */
+#define TK_SIZE 16 /* 128-bit temporal key */
+#define P1K_SIZE 10 /* 80-bit Phase1 key */
+#define RC4_KEY_SIZE 16 /* 128-bit RC4KEY (104 bits unknown) */
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256] = { /* Sbox for hash (can be in ROM) */
+{
+ 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+ 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+ 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+ 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+ 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+ 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+ 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+ 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+ 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+ 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+ 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+ 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+ 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+ 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+ 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+ 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+ 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+ 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+ 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+ 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+ 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+ 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+ 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+ 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+ 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+ 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+ 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+ 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+ 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+ 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+ 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+ 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+ },
+
+ { /* second half of table is unsigned char-reversed version of first! */
+ 0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+ 0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+ 0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+ 0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+ 0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+ 0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+ 0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+ 0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+ 0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+ 0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+ 0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+ 0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+ 0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+ 0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+ 0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+ 0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+ 0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+ 0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+ 0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+ 0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+ 0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+ 0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+ 0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+ 0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+ 0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+ 0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+ 0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+ 0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+ 0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+ 0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+ 0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+ 0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+ }
+};
+
+ /*
+**********************************************************************
+* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+*
+* Inputs:
+* tk[] = temporal key [128 bits]
+* ta[] = transmitter's MAC address [ 48 bits]
+* iv32 = upper 32 bits of IV [ 32 bits]
+* Output:
+* p1k[] = Phase 1 key [ 80 bits]
+*
+* Note:
+* This function only needs to be called every 2**16 packets,
+* although in theory it could be called every packet.
+*
+**********************************************************************
+*/
+static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
+{
+ int i;
+
+ /* Initialize the 80 bits of P1K[] from IV32 and TA[0..5] */
+ p1k[0] = Lo16(iv32);
+ p1k[1] = Hi16(iv32);
+ p1k[2] = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
+ p1k[3] = Mk16(ta[3], ta[2]);
+ p1k[4] = Mk16(ta[5], ta[4]);
+
+ /* Now compute an unbalanced Feistel cipher with 80-bit block */
+ /* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+ for (i = 0; i < PHASE1_LOOP_CNT; i++) { /* Each add operation here is mod 2**16 */
+ p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
+ p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
+ p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
+ p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
+ p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
+ p1k[4] += (unsigned short)i; /* avoid "slide attacks" */
+ }
+
+}
+
+/*
+**********************************************************************
+* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+*
+* Inputs:
+* tk[] = Temporal key [128 bits]
+* p1k[] = Phase 1 output key [ 80 bits]
+* iv16 = low 16 bits of IV counter [ 16 bits]
+* Output:
+* rc4key[] = the key used to encrypt the packet [128 bits]
+*
+* Note:
+* The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
+* across all packets using the same key TK value. Then, for a
+* given value of TK[], this TKIP48 construction guarantees that
+* the final RC4KEY value is unique across all packets.
+*
+* Suggested implementation optimization: if PPK[] is "overlaid"
+* appropriately on RC4KEY[], there is no need for the final
+* for loop below that copies the PPK[] result into RC4KEY[].
+*
+**********************************************************************
+*/
+static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
+{
+ int i;
+ u16 PPK[6]; /* temporary key for mixing */
+
+ /* Note: all adds in the PPK[] equations below are mod 2**16 */
+ for (i = 0; i < 5; i++)
+ PPK[i] = p1k[i]; /* first, copy P1K to PPK */
+ PPK[5] = p1k[4] + iv16; /* next, add in IV16 */
+
+ /* Bijective non-linear mixing of the 96 bits of PPK[0..5] */
+ PPK[0] += _S_(PPK[5] ^ TK16(0)); /* Mix key in each "round" */
+ PPK[1] += _S_(PPK[0] ^ TK16(1));
+ PPK[2] += _S_(PPK[1] ^ TK16(2));
+ PPK[3] += _S_(PPK[2] ^ TK16(3));
+ PPK[4] += _S_(PPK[3] ^ TK16(4));
+ PPK[5] += _S_(PPK[4] ^ TK16(5)); /* Total # S-box lookups == 6 */
+
+ /* Final sweep: bijective, "linear". Rotates kill LSB correlations */
+ PPK[0] += RotR1(PPK[5] ^ TK16(6));
+ PPK[1] += RotR1(PPK[0] ^ TK16(7)); /* Use all of TK[] in Phase2 */
+ PPK[2] += RotR1(PPK[1]);
+ PPK[3] += RotR1(PPK[2]);
+ PPK[4] += RotR1(PPK[3]);
+ PPK[5] += RotR1(PPK[4]);
+ /* Note: At this point, for a given key TK[0..15], the 96-bit output */
+ /* value PPK[0..5] is guaranteed to be unique, as a function */
+ /* of the 96-bit "input" value {TA, IV32, IV16}. That is, P1K */
+ /* is now a keyed permutation of {TA, IV32, IV16}. */
+
+ /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key */
+ rc4key[0] = Hi8(iv16); /* RC4KEY[0..2] is the WEP IV */
+ rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys */
+ rc4key[2] = Lo8(iv16);
+ rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+
+ /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15] (little-endian) */
+ for (i = 0; i < 6; i++) {
+ rc4key[4+2*i] = Lo8(PPK[i]);
+ rc4key[5+2*i] = Hi8(PPK[i]);
+ }
+
+}
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{ /* exclude ICV */
+ u16 pnl;
+ u32 pnh;
+ u8 rc4key[16];
+ u8 ttkey[16];
+ u8 crc[4];
+ u8 hw_hdr_offset = 0;
+ struct arc4context mycontext;
+ int curfragnum, length;
+
+ u8 *pframe, *payload, *iv, *prwskey;
+ union pn48 dot11txpn;
+ struct sta_info *stainfo;
+ struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ u32 res = _SUCCESS;
+
+ if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+ return _FAIL;
+
+ hw_hdr_offset = TXDESC_SIZE +
+ (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+ pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+ /* 4 start to encrypt each fragment */
+ if (pattrib->encrypt == _TKIP_) {
+ if (pattrib->psta)
+ stainfo = pattrib->psta;
+ else
+ stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
+
+ if (stainfo != NULL) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo!= NULL!!!\n"));
+
+ if (IS_MCAST(pattrib->ra))
+ prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+ else
+ prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+
+ for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+ iv = pframe+pattrib->hdrlen;
+ payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+
+ GET_TKIP_PN(iv, dot11txpn);
+
+ pnl = (u16)(dot11txpn.val);
+ pnh = (u32)(dot11txpn.val>>16);
+ phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
+ phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
+
+ if ((curfragnum+1) == pattrib->nr_frags) { /* 4 the last fragment */
+ length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+ RT_TRACE(_module_rtl871x_security_c_, _drv_info_,
+ ("pattrib->iv_len=%x, pattrib->icv_len=%x\n",
+ pattrib->iv_len, pattrib->icv_len));
+ *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
+
+ arcfour_init(&mycontext, rc4key, 16);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+ arcfour_encrypt(&mycontext, payload+length, crc, 4);
+ } else {
+ length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+ *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
+ arcfour_init(&mycontext, rc4key, 16);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+ arcfour_encrypt(&mycontext, payload+length, crc, 4);
+
+ pframe += pxmitpriv->frag_len;
+ pframe = (u8 *)RND4((size_t)(pframe));
+ }
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo==NULL!!!\n"));
+ res = _FAIL;
+ }
+ }
+
+ return res;
+}
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe)
+{ /* exclude ICV */
+ u16 pnl;
+ u32 pnh;
+ u8 rc4key[16];
+ u8 ttkey[16];
+ u8 crc[4];
+ struct arc4context mycontext;
+ int length;
+
+ u8 *pframe, *payload, *iv, *prwskey;
+ union pn48 dot11txpn;
+ struct sta_info *stainfo;
+ struct rx_pkt_attrib *prxattrib = &((struct recv_frame *)precvframe)->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ u32 res = _SUCCESS;
+
+ pframe = (unsigned char *)((struct recv_frame *)precvframe)->rx_data;
+
+ /* 4 start to decrypt recvframe */
+ if (prxattrib->encrypt == _TKIP_) {
+ stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+ if (stainfo != NULL) {
+ if (IS_MCAST(prxattrib->ra)) {
+ if (!psecuritypriv->binstallGrpkey) {
+ res = _FAIL;
+ DBG_88E("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
+ goto exit;
+ }
+ prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+ } else {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo!= NULL!!!\n"));
+ prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+ }
+
+ iv = pframe+prxattrib->hdrlen;
+ payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+ length = ((struct recv_frame *)precvframe)->len-prxattrib->hdrlen-prxattrib->iv_len;
+
+ GET_TKIP_PN(iv, dot11txpn);
+
+ pnl = (u16)(dot11txpn.val);
+ pnh = (u32)(dot11txpn.val>>16);
+
+ phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+ phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+ /* 4 decrypt payload include icv */
+
+ arcfour_init(&mycontext, rc4key, 16);
+ arcfour_encrypt(&mycontext, payload, payload, length);
+
+ *((__le32 *)crc) = getcrc32(payload, length-4);
+
+ if (crc[3] != payload[length-1] ||
+ crc[2] != payload[length-2] ||
+ crc[1] != payload[length-3] ||
+ crc[0] != payload[length-4]) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+ ("rtw_wep_decrypt:icv error crc (%4ph)!=payload (%4ph)\n",
+ &crc, &payload[length-4]));
+ res = _FAIL;
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo==NULL!!!\n"));
+ res = _FAIL;
+ }
+ }
+
+exit:
+ return res;
+}
+
+/* 3 ===== AES related ===== */
+
+#define MAX_MSG_SIZE 2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+static u8 sbox_table[256] = {
+ 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+ 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+ 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+ 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+ 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+ 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+ 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+ 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+ 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+ 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+ 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+ 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+ 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+ 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+ 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+ 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+ 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+ 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+ 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+ 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+ 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+ 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+ 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+ 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+ 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+ 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+ 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+ 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+ 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+ 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+ 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+ 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
+static void construct_mic_iv(u8 *mic_header1, int qc_exists, int a4_exists, u8 *mpdu, uint payload_length, u8 *pn_vector);
+static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu);
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists);
+static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c);
+static void xor_128(u8 *a, u8 *b, u8 *out);
+static void xor_32(u8 *a, u8 *b, u8 *out);
+static u8 sbox(u8 a);
+static void next_key(u8 *key, int round);
+static void byte_sub(u8 *in, u8 *out);
+static void shift_row(u8 *in, u8 *out);
+static void mix_column(u8 *in, u8 *out);
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
+
+/****************************************/
+/* aes128k128d() */
+/* Performs a 128 bit AES encrypt with */
+/* 128 bit data. */
+/****************************************/
+static void xor_128(u8 *a, u8 *b, u8 *out)
+{
+ int i;
+
+ for (i = 0; i < 16; i++)
+ out[i] = a[i] ^ b[i];
+
+}
+
+static void xor_32(u8 *a, u8 *b, u8 *out)
+{
+ int i;
+
+ for (i = 0; i < 4; i++)
+ out[i] = a[i] ^ b[i];
+
+}
+
+static u8 sbox(u8 a)
+{
+ return sbox_table[(int)a];
+}
+
+static void next_key(u8 *key, int round)
+{
+ u8 rcon;
+ u8 sbox_key[4];
+ u8 rcon_table[12] = {
+ 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+ 0x1b, 0x36, 0x36, 0x36
+ };
+
+ sbox_key[0] = sbox(key[13]);
+ sbox_key[1] = sbox(key[14]);
+ sbox_key[2] = sbox(key[15]);
+ sbox_key[3] = sbox(key[12]);
+
+ rcon = rcon_table[round];
+
+ xor_32(&key[0], sbox_key, &key[0]);
+ key[0] = key[0] ^ rcon;
+
+ xor_32(&key[4], &key[0], &key[4]);
+ xor_32(&key[8], &key[4], &key[8]);
+ xor_32(&key[12], &key[8], &key[12]);
+
+}
+
+static void byte_sub(u8 *in, u8 *out)
+{
+ int i;
+
+ for (i = 0; i < 16; i++)
+ out[i] = sbox(in[i]);
+
+}
+
+static void shift_row(u8 *in, u8 *out)
+{
+
+ out[0] = in[0];
+ out[1] = in[5];
+ out[2] = in[10];
+ out[3] = in[15];
+ out[4] = in[4];
+ out[5] = in[9];
+ out[6] = in[14];
+ out[7] = in[3];
+ out[8] = in[8];
+ out[9] = in[13];
+ out[10] = in[2];
+ out[11] = in[7];
+ out[12] = in[12];
+ out[13] = in[1];
+ out[14] = in[6];
+ out[15] = in[11];
+
+}
+
+static void mix_column(u8 *in, u8 *out)
+{
+ int i;
+ u8 add1b[4];
+ u8 add1bf7[4];
+ u8 rotl[4];
+ u8 swap_halfs[4];
+ u8 andf7[4];
+ u8 rotr[4];
+ u8 temp[4];
+ u8 tempb[4];
+
+ for (i = 0 ; i < 4; i++) {
+ if ((in[i] & 0x80) == 0x80)
+ add1b[i] = 0x1b;
+ else
+ add1b[i] = 0x00;
+ }
+
+ swap_halfs[0] = in[2]; /* Swap halves */
+ swap_halfs[1] = in[3];
+ swap_halfs[2] = in[0];
+ swap_halfs[3] = in[1];
+
+ rotl[0] = in[3]; /* Rotate left 8 bits */
+ rotl[1] = in[0];
+ rotl[2] = in[1];
+ rotl[3] = in[2];
+
+ andf7[0] = in[0] & 0x7f;
+ andf7[1] = in[1] & 0x7f;
+ andf7[2] = in[2] & 0x7f;
+ andf7[3] = in[3] & 0x7f;
+
+ for (i = 3; i > 0; i--) { /* logical shift left 1 bit */
+ andf7[i] = andf7[i] << 1;
+ if ((andf7[i-1] & 0x80) == 0x80)
+ andf7[i] = (andf7[i] | 0x01);
+ }
+ andf7[0] = andf7[0] << 1;
+ andf7[0] = andf7[0] & 0xfe;
+
+ xor_32(add1b, andf7, add1bf7);
+
+ xor_32(in, add1bf7, rotr);
+
+ temp[0] = rotr[0]; /* Rotate right 8 bits */
+ rotr[0] = rotr[1];
+ rotr[1] = rotr[2];
+ rotr[2] = rotr[3];
+ rotr[3] = temp[0];
+
+ xor_32(add1bf7, rotr, temp);
+ xor_32(swap_halfs, rotl, tempb);
+ xor_32(temp, tempb, out);
+
+}
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+ int round;
+ int i;
+ u8 intermediatea[16];
+ u8 intermediateb[16];
+ u8 round_key[16];
+
+ for (i = 0; i < 16; i++)
+ round_key[i] = key[i];
+ for (round = 0; round < 11; round++) {
+ if (round == 0) {
+ xor_128(round_key, data, ciphertext);
+ next_key(round_key, round);
+ } else if (round == 10) {
+ byte_sub(ciphertext, intermediatea);
+ shift_row(intermediatea, intermediateb);
+ xor_128(intermediateb, round_key, ciphertext);
+ } else { /* 1 - 9 */
+ byte_sub(ciphertext, intermediatea);
+ shift_row(intermediatea, intermediateb);
+ mix_column(&intermediateb[0], &intermediatea[0]);
+ mix_column(&intermediateb[4], &intermediatea[4]);
+ mix_column(&intermediateb[8], &intermediatea[8]);
+ mix_column(&intermediateb[12], &intermediatea[12]);
+ xor_128(intermediatea, round_key, ciphertext);
+ next_key(round_key, round);
+ }
+ }
+
+}
+
+/************************************************/
+/* construct_mic_iv() */
+/* Builds the MIC IV from header fields and PN */
+/************************************************/
+static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
+ uint payload_length, u8 *pn_vector)
+{
+ int i;
+
+ mic_iv[0] = 0x59;
+ if (qc_exists && a4_exists)
+ mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
+ if (qc_exists && !a4_exists)
+ mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
+ if (!qc_exists)
+ mic_iv[1] = 0x00;
+ for (i = 2; i < 8; i++)
+ mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+ for (i = 8; i < 14; i++)
+ mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
+ mic_iv[14] = (unsigned char) (payload_length / 256);
+ mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+/************************************************/
+/* construct_mic_header1() */
+/* Builds the first MIC header block from */
+/* header fields. */
+/************************************************/
+static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
+{
+
+ mic_header1[0] = (u8)((header_length - 2) / 256);
+ mic_header1[1] = (u8)((header_length - 2) % 256);
+ mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
+ mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
+ mic_header1[4] = mpdu[4]; /* A1 */
+ mic_header1[5] = mpdu[5];
+ mic_header1[6] = mpdu[6];
+ mic_header1[7] = mpdu[7];
+ mic_header1[8] = mpdu[8];
+ mic_header1[9] = mpdu[9];
+ mic_header1[10] = mpdu[10]; /* A2 */
+ mic_header1[11] = mpdu[11];
+ mic_header1[12] = mpdu[12];
+ mic_header1[13] = mpdu[13];
+ mic_header1[14] = mpdu[14];
+ mic_header1[15] = mpdu[15];
+
+}
+
+/************************************************/
+/* construct_mic_header2() */
+/* Builds the last MIC header block from */
+/* header fields. */
+/************************************************/
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists)
+{
+ int i;
+
+ for (i = 0; i < 16; i++)
+ mic_header2[i] = 0x00;
+
+ mic_header2[0] = mpdu[16]; /* A3 */
+ mic_header2[1] = mpdu[17];
+ mic_header2[2] = mpdu[18];
+ mic_header2[3] = mpdu[19];
+ mic_header2[4] = mpdu[20];
+ mic_header2[5] = mpdu[21];
+
+ mic_header2[6] = 0x00;
+ mic_header2[7] = 0x00; /* mpdu[23]; */
+
+ if (!qc_exists && a4_exists) {
+ for (i = 0; i < 6; i++)
+ mic_header2[8+i] = mpdu[24+i]; /* A4 */
+ }
+
+ if (qc_exists && !a4_exists) {
+ mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+ mic_header2[9] = mpdu[25] & 0x00;
+ }
+
+ if (qc_exists && a4_exists) {
+ for (i = 0; i < 6; i++)
+ mic_header2[8+i] = mpdu[24+i]; /* A4 */
+
+ mic_header2[14] = mpdu[30] & 0x0f;
+ mic_header2[15] = mpdu[31] & 0x00;
+ }
+
+}
+
+/************************************************/
+/* construct_mic_header2() */
+/* Builds the last MIC header block from */
+/* header fields. */
+/************************************************/
+static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c)
+{
+ int i;
+
+ for (i = 0; i < 16; i++)
+ ctr_preload[i] = 0x00;
+ i = 0;
+
+ ctr_preload[0] = 0x01; /* flag */
+ if (qc_exists && a4_exists)
+ ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
+ if (qc_exists && !a4_exists)
+ ctr_preload[1] = mpdu[24] & 0x0f;
+
+ for (i = 2; i < 8; i++)
+ ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+ for (i = 8; i < 14; i++)
+ ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
+ ctr_preload[14] = (unsigned char) (c / 256); /* Ctr */
+ ctr_preload[15] = (unsigned char) (c % 256);
+
+}
+
+/************************************/
+/* bitwise_xor() */
+/* A 128 bit, bitwise exclusive or */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+ int i;
+
+ for (i = 0; i < 16; i++)
+ out[i] = ina[i] ^ inb[i];
+
+}
+
+static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
+{
+ uint qc_exists, a4_exists, i, j, payload_remainder,
+ num_blocks, payload_index;
+
+ u8 pn_vector[6];
+ u8 mic_iv[16];
+ u8 mic_header1[16];
+ u8 mic_header2[16];
+ u8 ctr_preload[16];
+
+ /* Intermediate Buffers */
+ u8 chain_buffer[16];
+ u8 aes_out[16];
+ u8 padded_buffer[16];
+ u8 mic[8];
+ uint frtype = GetFrameType(pframe);
+ uint frsubtype = GetFrameSubType(pframe);
+
+ frsubtype = frsubtype>>4;
+
+ memset((void *)mic_iv, 0, 16);
+ memset((void *)mic_header1, 0, 16);
+ memset((void *)mic_header2, 0, 16);
+ memset((void *)ctr_preload, 0, 16);
+ memset((void *)chain_buffer, 0, 16);
+ memset((void *)aes_out, 0, 16);
+ memset((void *)padded_buffer, 0, 16);
+
+ if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN))
+ a4_exists = 0;
+ else
+ a4_exists = 1;
+
+ if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) || (frtype == WIFI_DATA_CFACKPOLL)) {
+ qc_exists = 1;
+ if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+ hdrlen += 2;
+ } else if ((frsubtype == 0x08) || (frsubtype == 0x09) || (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
+ if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+ hdrlen += 2;
+ qc_exists = 1;
+ } else {
+ qc_exists = 0;
+ }
+
+ pn_vector[0] = pframe[hdrlen];
+ pn_vector[1] = pframe[hdrlen+1];
+ pn_vector[2] = pframe[hdrlen+4];
+ pn_vector[3] = pframe[hdrlen+5];
+ pn_vector[4] = pframe[hdrlen+6];
+ pn_vector[5] = pframe[hdrlen+7];
+
+ construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
+
+ construct_mic_header1(mic_header1, hdrlen, pframe);
+ construct_mic_header2(mic_header2, pframe, a4_exists, qc_exists);
+
+ payload_remainder = plen % 16;
+ num_blocks = plen / 16;
+
+ /* Find start of payload */
+ payload_index = (hdrlen + 8);
+
+ /* Calculate MIC */
+ aes128k128d(key, mic_iv, aes_out);
+ bitwise_xor(aes_out, mic_header1, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+ bitwise_xor(aes_out, mic_header2, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+
+ for (i = 0; i < num_blocks; i++) {
+ bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
+
+ payload_index += 16;
+ aes128k128d(key, chain_buffer, aes_out);
+ }
+
+ /* Add on the final payload block if it needs padding */
+ if (payload_remainder > 0) {
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < payload_remainder; j++)
+ padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+ }
+
+ for (j = 0; j < 8; j++)
+ mic[j] = aes_out[j];
+
+ /* Insert MIC into payload */
+ for (j = 0; j < 8; j++)
+ pframe[payload_index+j] = mic[j]; /* message[payload_index+j] = mic[j]; */
+
+ payload_index = hdrlen + 8;
+ for (i = 0; i < num_blocks; i++) {
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i+1);
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+ for (j = 0; j < 16; j++)
+ pframe[payload_index++] = chain_buffer[j];
+ }
+
+ if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/
+ /* encrypt it and copy the unpadded part back */
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks+1);
+
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < payload_remainder; j++)
+ padded_buffer[j] = pframe[payload_index+j];
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ for (j = 0; j < payload_remainder; j++)
+ pframe[payload_index++] = chain_buffer[j];
+ }
+ /* Encrypt the MIC */
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, 0);
+
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < 8; j++)
+ padded_buffer[j] = pframe[j+hdrlen+8+plen];
+
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ for (j = 0; j < 8; j++)
+ pframe[payload_index++] = chain_buffer[j];
+
+ return _SUCCESS;
+}
+
+u32 rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{ /* exclude ICV */
+
+ /*static*/
+/* unsigned char message[MAX_MSG_SIZE]; */
+
+ /* Intermediate Buffers */
+ int curfragnum, length;
+ u8 *pframe, *prwskey; /* *payload,*iv */
+ u8 hw_hdr_offset = 0;
+ struct sta_info *stainfo;
+ struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+/* uint offset = 0; */
+ u32 res = _SUCCESS;
+
+ if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+ return _FAIL;
+
+ hw_hdr_offset = TXDESC_SIZE +
+ (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+
+ pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+ /* 4 start to encrypt each fragment */
+ if ((pattrib->encrypt == _AES_)) {
+ if (pattrib->psta)
+ stainfo = pattrib->psta;
+ else
+ stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
+
+ if (stainfo != NULL) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo!= NULL!!!\n"));
+
+ if (IS_MCAST(pattrib->ra))
+ prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+ else
+ prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+ for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+ if ((curfragnum+1) == pattrib->nr_frags) { /* 4 the last fragment */
+ length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+
+ aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+ } else{
+ length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+
+ aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+ pframe += pxmitpriv->frag_len;
+ pframe = (u8 *)RND4((size_t)(pframe));
+ }
+ }
+ } else{
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo==NULL!!!\n"));
+ res = _FAIL;
+ }
+ }
+
+ return res;
+}
+
+static int aes_decipher(u8 *key, uint hdrlen,
+ u8 *pframe, uint plen)
+{
+ static u8 message[MAX_MSG_SIZE];
+ uint qc_exists, a4_exists, i, j, payload_remainder,
+ num_blocks, payload_index;
+ int res = _SUCCESS;
+ u8 pn_vector[6];
+ u8 mic_iv[16];
+ u8 mic_header1[16];
+ u8 mic_header2[16];
+ u8 ctr_preload[16];
+
+ /* Intermediate Buffers */
+ u8 chain_buffer[16];
+ u8 aes_out[16];
+ u8 padded_buffer[16];
+ u8 mic[8];
+
+/* uint offset = 0; */
+ uint frtype = GetFrameType(pframe);
+ uint frsubtype = GetFrameSubType(pframe);
+
+ frsubtype = frsubtype>>4;
+
+ memset((void *)mic_iv, 0, 16);
+ memset((void *)mic_header1, 0, 16);
+ memset((void *)mic_header2, 0, 16);
+ memset((void *)ctr_preload, 0, 16);
+ memset((void *)chain_buffer, 0, 16);
+ memset((void *)aes_out, 0, 16);
+ memset((void *)padded_buffer, 0, 16);
+
+ /* start to decrypt the payload */
+
+ num_blocks = (plen-8) / 16; /* plen including llc, payload_length and mic) */
+
+ payload_remainder = (plen-8) % 16;
+
+ pn_vector[0] = pframe[hdrlen];
+ pn_vector[1] = pframe[hdrlen+1];
+ pn_vector[2] = pframe[hdrlen+4];
+ pn_vector[3] = pframe[hdrlen+5];
+ pn_vector[4] = pframe[hdrlen+6];
+ pn_vector[5] = pframe[hdrlen+7];
+
+ if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN))
+ a4_exists = 0;
+ else
+ a4_exists = 1;
+
+ if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) ||
+ (frtype == WIFI_DATA_CFACKPOLL)) {
+ qc_exists = 1;
+ if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+ hdrlen += 2;
+ } else if ((frsubtype == 0x08) || (frsubtype == 0x09) ||
+ (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
+ if (hdrlen != WLAN_HDR_A3_QOS_LEN)
+ hdrlen += 2;
+ qc_exists = 1;
+ } else {
+ qc_exists = 0;
+ }
+
+ /* now, decrypt pframe with hdrlen offset and plen long */
+
+ payload_index = hdrlen + 8; /* 8 is for extiv */
+
+ for (i = 0; i < num_blocks; i++) {
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i+1);
+
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+ for (j = 0; j < 16; j++)
+ pframe[payload_index++] = chain_buffer[j];
+ }
+
+ if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/
+ /* encrypt it and copy the unpadded part back */
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks+1);
+
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < payload_remainder; j++)
+ padded_buffer[j] = pframe[payload_index+j];
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ for (j = 0; j < payload_remainder; j++)
+ pframe[payload_index++] = chain_buffer[j];
+ }
+
+ /* start to calculate the mic */
+ if ((hdrlen+plen+8) <= MAX_MSG_SIZE)
+ memcpy(message, pframe, (hdrlen + plen+8)); /* 8 is for ext iv len */
+
+ pn_vector[0] = pframe[hdrlen];
+ pn_vector[1] = pframe[hdrlen+1];
+ pn_vector[2] = pframe[hdrlen+4];
+ pn_vector[3] = pframe[hdrlen+5];
+ pn_vector[4] = pframe[hdrlen+6];
+ pn_vector[5] = pframe[hdrlen+7];
+ construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen-8, pn_vector);
+
+ construct_mic_header1(mic_header1, hdrlen, message);
+ construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
+
+ payload_remainder = (plen-8) % 16;
+ num_blocks = (plen-8) / 16;
+
+ /* Find start of payload */
+ payload_index = (hdrlen + 8);
+
+ /* Calculate MIC */
+ aes128k128d(key, mic_iv, aes_out);
+ bitwise_xor(aes_out, mic_header1, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+ bitwise_xor(aes_out, mic_header2, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+
+ for (i = 0; i < num_blocks; i++) {
+ bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+ payload_index += 16;
+ aes128k128d(key, chain_buffer, aes_out);
+ }
+
+ /* Add on the final payload block if it needs padding */
+ if (payload_remainder > 0) {
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < payload_remainder; j++)
+ padded_buffer[j] = message[payload_index++];
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ aes128k128d(key, chain_buffer, aes_out);
+ }
+
+ for (j = 0 ; j < 8; j++)
+ mic[j] = aes_out[j];
+
+ /* Insert MIC into payload */
+ for (j = 0; j < 8; j++)
+ message[payload_index+j] = mic[j];
+
+ payload_index = hdrlen + 8;
+ for (i = 0; i < num_blocks; i++) {
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i+1);
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+ for (j = 0; j < 16; j++)
+ message[payload_index++] = chain_buffer[j];
+ }
+
+ if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/
+ /* encrypt it and copy the unpadded part back */
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, num_blocks+1);
+
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < payload_remainder; j++)
+ padded_buffer[j] = message[payload_index+j];
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ for (j = 0; j < payload_remainder; j++)
+ message[payload_index++] = chain_buffer[j];
+ }
+
+ /* Encrypt the MIC */
+ construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, 0);
+
+ for (j = 0; j < 16; j++)
+ padded_buffer[j] = 0x00;
+ for (j = 0; j < 8; j++)
+ padded_buffer[j] = message[j+hdrlen+8+plen-8];
+
+ aes128k128d(key, ctr_preload, aes_out);
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ for (j = 0; j < 8; j++)
+ message[payload_index++] = chain_buffer[j];
+
+ /* compare the mic */
+ for (i = 0; i < 8; i++) {
+ if (pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i]) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+ ("aes_decipher:mic check error mic[%d]: pframe(%x)!=message(%x)\n",
+ i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
+ DBG_88E("aes_decipher:mic check error mic[%d]: pframe(%x)!=message(%x)\n",
+ i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]);
+ res = _FAIL;
+ }
+ }
+
+ return res;
+}
+
+u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
+{ /* exclude ICV */
+ /* Intermediate Buffers */
+ int length;
+ u8 *pframe, *prwskey; /* *payload,*iv */
+ struct sta_info *stainfo;
+ struct rx_pkt_attrib *prxattrib = &((struct recv_frame *)precvframe)->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ u32 res = _SUCCESS;
+
+ pframe = (unsigned char *)((struct recv_frame *)precvframe)->rx_data;
+ /* 4 start to encrypt each fragment */
+ if ((prxattrib->encrypt == _AES_)) {
+ stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+ if (stainfo != NULL) {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n"));
+
+ if (IS_MCAST(prxattrib->ra)) {
+ /* in concurrent we should use sw descrypt in group key, so we remove this message */
+ if (!psecuritypriv->binstallGrpkey) {
+ res = _FAIL;
+ DBG_88E("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
+ goto exit;
+ }
+ prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+ if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
+ DBG_88E("not match packet_index=%d, install_index=%d\n",
+ prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
+ res = _FAIL;
+ goto exit;
+ }
+ } else {
+ prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+ }
+ length = ((struct recv_frame *)precvframe)->len-prxattrib->hdrlen-prxattrib->iv_len;
+ res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
+ } else {
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo==NULL!!!\n"));
+ res = _FAIL;
+ }
+ }
+
+exit:
+ return res;
+}
+
+/* AES tables*/
+const u32 Te0[256] = {
+ 0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
+ 0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
+ 0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
+ 0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
+ 0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
+ 0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
+ 0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
+ 0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
+ 0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
+ 0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
+ 0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
+ 0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
+ 0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
+ 0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
+ 0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
+ 0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
+ 0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
+ 0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
+ 0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
+ 0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
+ 0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
+ 0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
+ 0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
+ 0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
+ 0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
+ 0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
+ 0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
+ 0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
+ 0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
+ 0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
+ 0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
+ 0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
+ 0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
+ 0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
+ 0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
+ 0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
+ 0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
+ 0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
+ 0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
+ 0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
+ 0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
+ 0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
+ 0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
+ 0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
+ 0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
+ 0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
+ 0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
+ 0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
+ 0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
+ 0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
+ 0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
+ 0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
+ 0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
+ 0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
+ 0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
+ 0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
+ 0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
+ 0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
+ 0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
+ 0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
+ 0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
+ 0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
+ 0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
+ 0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
+};
+
+const u32 Td0[256] = {
+ 0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
+ 0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
+ 0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
+ 0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
+ 0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
+ 0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
+ 0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
+ 0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
+ 0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
+ 0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
+ 0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
+ 0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
+ 0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
+ 0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
+ 0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
+ 0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
+ 0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
+ 0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
+ 0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
+ 0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
+ 0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
+ 0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
+ 0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
+ 0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
+ 0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
+ 0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
+ 0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
+ 0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
+ 0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
+ 0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
+ 0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
+ 0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
+ 0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
+ 0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
+ 0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
+ 0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
+ 0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
+ 0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
+ 0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
+ 0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
+ 0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
+ 0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
+ 0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
+ 0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
+ 0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
+ 0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
+ 0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
+ 0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
+ 0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
+ 0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
+ 0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
+ 0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
+ 0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
+ 0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
+ 0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
+ 0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
+ 0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
+ 0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
+ 0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
+ 0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
+ 0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
+ 0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
+ 0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
+ 0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
+};
+
+const u8 Td4s[256] = {
+ 0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
+ 0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
+ 0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
+ 0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
+ 0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
+ 0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
+ 0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
+ 0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
+ 0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
+ 0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
+ 0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
+ 0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
+ 0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
+ 0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
+ 0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
+ 0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
+ 0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
+ 0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
+ 0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
+ 0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
+ 0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
+ 0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
+ 0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
+ 0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
+ 0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
+ 0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
+ 0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
+ 0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
+ 0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
+ 0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
+ 0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
+ 0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
+};
+const u8 rcons[] = {
+ 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
+ /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+};
+
+/**
+ * Expand the cipher key into the encryption key schedule.
+ *
+ * @return the number of rounds for the given cipher key size.
+ */
+#define ROUND(i, d, s) \
+do { \
+ d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
+ d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
+ d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
+ d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]; \
+} while (0);
+
+/**
+ * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC)
+ * @key: 128-bit key for the hash operation
+ * @data: Data buffer for which a MAC is determined
+ * @data_len: Length of data buffer in bytes
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */
+void rtw_use_tkipkey_handler(void *FunctionContext)
+{
+ struct adapter *padapter = (struct adapter *)FunctionContext;
+
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler ^^^\n"));
+
+ padapter->securitypriv.busetkipkey = true;
+
+ RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler padapter->securitypriv.busetkipkey=%d^^^\n", padapter->securitypriv.busetkipkey));
+
+}
diff --git a/drivers/staging/r8188eu/core/rtw_sreset.c b/drivers/staging/r8188eu/core/rtw_sreset.c
new file mode 100644
index 000000000000..298f75400c8f
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_sreset.c
@@ -0,0 +1,79 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#include <rtw_sreset.h>
+
+void sreset_init_value(struct adapter *padapter)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
+ struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+ _rtw_mutex_init(&psrtpriv->silentreset_mutex);
+ psrtpriv->silent_reset_inprogress = false;
+ psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+ psrtpriv->last_tx_time = 0;
+ psrtpriv->last_tx_complete_time = 0;
+}
+void sreset_reset_value(struct adapter *padapter)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
+ struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+ psrtpriv->silent_reset_inprogress = false;
+ psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+ psrtpriv->last_tx_time = 0;
+ psrtpriv->last_tx_complete_time = 0;
+}
+
+u8 sreset_get_wifi_status(struct adapter *padapter)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
+ struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+ u8 status = WIFI_STATUS_SUCCESS;
+ u32 val32 = 0;
+
+ if (psrtpriv->silent_reset_inprogress)
+ return status;
+ val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
+ if (val32 == 0xeaeaeaea) {
+ psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;
+ } else if (val32 != 0) {
+ DBG_88E("txdmastatu(%x)\n", val32);
+ psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR;
+ }
+
+ if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) {
+ DBG_88E("==>%s error_status(0x%x)\n", __func__, psrtpriv->Wifi_Error_Status);
+ status = (psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL|USB_WRITE_PORT_FAIL)));
+ }
+ DBG_88E("==> %s wifi_status(0x%x)\n", __func__, status);
+
+ /* status restore */
+ psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+
+ return status;
+}
+
+void sreset_set_wifi_error_status(struct adapter *padapter, u32 status)
+{
+ struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
+ pHalData->srestpriv.Wifi_Error_Status = status;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_sta_mgt.c b/drivers/staging/r8188eu/core/rtw_sta_mgt.c
new file mode 100644
index 000000000000..30a1dd369112
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_sta_mgt.c
@@ -0,0 +1,609 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_STA_MGT_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+#include <mlme_osdep.h>
+#include <sta_info.h>
+
+static void _rtw_init_stainfo(struct sta_info *psta)
+{
+
+ memset((u8 *)psta, 0, sizeof (struct sta_info));
+
+ spin_lock_init(&psta->lock);
+ INIT_LIST_HEAD(&psta->list);
+ INIT_LIST_HEAD(&psta->hash_list);
+ _rtw_init_queue(&psta->sleep_q);
+ psta->sleepq_len = 0;
+
+ _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+ _rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+#ifdef CONFIG_88EU_AP_MODE
+
+ INIT_LIST_HEAD(&psta->asoc_list);
+
+ INIT_LIST_HEAD(&psta->auth_list);
+
+ psta->expire_to = 0;
+
+ psta->flags = 0;
+
+ psta->capability = 0;
+
+ psta->bpairwise_key_installed = false;
+
+#ifdef CONFIG_88EU_AP_MODE
+ psta->nonerp_set = 0;
+ psta->no_short_slot_time_set = 0;
+ psta->no_short_preamble_set = 0;
+ psta->no_ht_gf_set = 0;
+ psta->no_ht_set = 0;
+ psta->ht_20mhz_set = 0;
+#endif
+
+ psta->under_exist_checking = 0;
+
+ psta->keep_alive_trycnt = 0;
+
+#endif /* CONFIG_88EU_AP_MODE */
+
+}
+
+u32 _rtw_init_sta_priv(struct sta_priv *pstapriv)
+{
+ struct sta_info *psta;
+ s32 i;
+
+ pstapriv->pallocated_stainfo_buf = rtw_zvmalloc(sizeof(struct sta_info) * NUM_STA + 4);
+
+ if (!pstapriv->pallocated_stainfo_buf)
+ return _FAIL;
+
+ pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+ ((size_t)(pstapriv->pallocated_stainfo_buf) & 3);
+
+ _rtw_init_queue(&pstapriv->free_sta_queue);
+
+ spin_lock_init(&pstapriv->sta_hash_lock);
+
+ pstapriv->asoc_sta_count = 0;
+ _rtw_init_queue(&pstapriv->sleep_q);
+ _rtw_init_queue(&pstapriv->wakeup_q);
+
+ psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+ for (i = 0; i < NUM_STA; i++) {
+ _rtw_init_stainfo(psta);
+
+ INIT_LIST_HEAD(&(pstapriv->sta_hash[i]));
+
+ list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+ psta++;
+ }
+
+#ifdef CONFIG_88EU_AP_MODE
+
+ pstapriv->sta_dz_bitmap = 0;
+ pstapriv->tim_bitmap = 0;
+
+ INIT_LIST_HEAD(&pstapriv->asoc_list);
+ INIT_LIST_HEAD(&pstapriv->auth_list);
+ spin_lock_init(&pstapriv->asoc_list_lock);
+ spin_lock_init(&pstapriv->auth_list_lock);
+ pstapriv->asoc_list_cnt = 0;
+ pstapriv->auth_list_cnt = 0;
+
+ pstapriv->auth_to = 3; /* 3*2 = 6 sec */
+ pstapriv->assoc_to = 3;
+ pstapriv->expire_to = 3; /* 3*2 = 6 sec */
+ pstapriv->max_num_sta = NUM_STA;
+#endif
+
+ return _SUCCESS;
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+ int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
+
+ if (!stainfo_offset_valid(offset))
+ DBG_88E("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+ return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+ if (!stainfo_offset_valid(offset))
+ DBG_88E("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+ return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+void _rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv);
+void _rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv)
+{
+
+ _rtw_spinlock_free(&psta_xmitpriv->lock);
+
+ _rtw_spinlock_free(&(psta_xmitpriv->be_q.sta_pending.lock));
+ _rtw_spinlock_free(&(psta_xmitpriv->bk_q.sta_pending.lock));
+ _rtw_spinlock_free(&(psta_xmitpriv->vi_q.sta_pending.lock));
+ _rtw_spinlock_free(&(psta_xmitpriv->vo_q.sta_pending.lock));
+
+}
+
+static void _rtw_free_sta_recv_priv_lock(struct sta_recv_priv *psta_recvpriv)
+{
+
+ _rtw_spinlock_free(&psta_recvpriv->lock);
+
+ _rtw_spinlock_free(&(psta_recvpriv->defrag_q.lock));
+
+}
+
+void rtw_mfree_stainfo(struct sta_info *psta);
+void rtw_mfree_stainfo(struct sta_info *psta)
+{
+
+ if (&psta->lock != NULL)
+ _rtw_spinlock_free(&psta->lock);
+
+ _rtw_free_sta_xmit_priv_lock(&psta->sta_xmitpriv);
+ _rtw_free_sta_recv_priv_lock(&psta->sta_recvpriv);
+
+}
+
+/* this function is used to free the memory of lock || sema for all stainfos */
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv);
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
+{
+ struct list_head *plist, *phead;
+ struct sta_info *psta = NULL;
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+
+ phead = get_list_head(&pstapriv->free_sta_queue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info , list);
+ plist = plist->next;
+ }
+
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+static void rtw_mfree_sta_priv_lock(struct sta_priv *pstapriv)
+{
+#ifdef CONFIG_88EU_AP_MODE
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+#endif
+
+ rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
+
+ _rtw_spinlock_free(&pstapriv->free_sta_queue.lock);
+
+ _rtw_spinlock_free(&pstapriv->sta_hash_lock);
+ _rtw_spinlock_free(&pstapriv->wakeup_q.lock);
+ _rtw_spinlock_free(&pstapriv->sleep_q.lock);
+
+#ifdef CONFIG_88EU_AP_MODE
+ _rtw_spinlock_free(&pstapriv->asoc_list_lock);
+ _rtw_spinlock_free(&pstapriv->auth_list_lock);
+ _rtw_spinlock_free(&pacl_list->acl_node_q.lock);
+#endif
+}
+
+u32 _rtw_free_sta_priv(struct sta_priv *pstapriv)
+{
+ struct list_head *phead, *plist;
+ struct sta_info *psta = NULL;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ int index;
+
+ if (pstapriv) {
+ /* delete all reordering_ctrl_timer */
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+ for (index = 0; index < NUM_STA; index++) {
+ phead = &(pstapriv->sta_hash[index]);
+ plist = phead->next;
+
+ while (phead != plist) {
+ int i;
+ psta = container_of(plist, struct sta_info , hash_list);
+ plist = plist->next;
+
+ for (i = 0; i < 16; i++) {
+ preorder_ctrl = &psta->recvreorder_ctrl[i];
+ _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+ }
+ }
+ }
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+ /*===============================*/
+
+ rtw_mfree_sta_priv_lock(pstapriv);
+
+ if (pstapriv->pallocated_stainfo_buf)
+ rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info)*NUM_STA+4);
+ }
+
+ return _SUCCESS;
+}
+
+struct sta_info *rtw_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+ s32 index;
+ struct list_head *phash_list;
+ struct sta_info *psta;
+ struct __queue *pfree_sta_queue;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ int i = 0;
+ u16 wRxSeqInitialValue = 0xffff;
+
+ pfree_sta_queue = &pstapriv->free_sta_queue;
+
+ spin_lock_bh(&pfree_sta_queue->lock);
+
+ if (list_empty(&pfree_sta_queue->queue)) {
+ spin_unlock_bh(&pfree_sta_queue->lock);
+ psta = NULL;
+ } else {
+ psta = container_of((&pfree_sta_queue->queue)->next, struct sta_info, list);
+ list_del_init(&(psta->list));
+ spin_unlock_bh(&pfree_sta_queue->lock);
+ _rtw_init_stainfo(psta);
+ memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+ index = wifi_mac_hash(hwaddr);
+ RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_, ("rtw_alloc_stainfo: index=%x", index));
+ if (index >= NUM_STA) {
+ RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("ERROR => rtw_alloc_stainfo: index >= NUM_STA"));
+ psta = NULL;
+ goto exit;
+ }
+ phash_list = &(pstapriv->sta_hash[index]);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+
+ list_add_tail(&psta->hash_list, phash_list);
+
+ pstapriv->asoc_sta_count++ ;
+
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+/* Commented by Albert 2009/08/13 */
+/* For the SMC router, the sequence number of first packet of WPS handshake will be 0. */
+/* In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable. */
+/* So, we initialize the tid_rxseq variable as the 0xffff. */
+
+ for (i = 0; i < 16; i++)
+ memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2);
+
+ RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_,
+ ("alloc number_%d stainfo with hwaddr = %pM\n",
+ pstapriv->asoc_sta_count , hwaddr));
+
+ init_addba_retry_timer(pstapriv->padapter, psta);
+
+ /* for A-MPDU Rx reordering buffer control */
+ for (i = 0; i < 16; i++) {
+ preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+ preorder_ctrl->padapter = pstapriv->padapter;
+
+ preorder_ctrl->enable = false;
+
+ preorder_ctrl->indicate_seq = 0xffff;
+ preorder_ctrl->wend_b = 0xffff;
+ preorder_ctrl->wsize_b = 64;/* 64; */
+
+ _rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+ rtw_init_recv_timer(preorder_ctrl);
+ }
+
+ /* init for DM */
+ psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+ psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+
+ /* init for the sequence number of received management frame */
+ psta->RxMgmtFrameSeqNum = 0xffff;
+ }
+
+exit:
+
+ return psta;
+}
+
+/* using pstapriv->sta_hash_lock to protect */
+u32 rtw_free_stainfo(struct adapter *padapter , struct sta_info *psta)
+{
+ int i;
+ struct __queue *pfree_sta_queue;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ struct sta_xmit_priv *pstaxmitpriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ if (psta == NULL)
+ goto exit;
+
+ pfree_sta_queue = &pstapriv->free_sta_queue;
+
+ pstaxmitpriv = &psta->sta_xmitpriv;
+
+ spin_lock_bh(&pxmitpriv->lock);
+
+ rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+ psta->sleepq_len = 0;
+
+ rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+
+ list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+
+ rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+
+ list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+
+ rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+
+ list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+
+ rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+
+ list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+
+ spin_unlock_bh(&pxmitpriv->lock);
+
+ list_del_init(&psta->hash_list);
+ RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("\n free number_%d stainfo with hwaddr=0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n", pstapriv->asoc_sta_count , psta->hwaddr[0], psta->hwaddr[1], psta->hwaddr[2], psta->hwaddr[3], psta->hwaddr[4], psta->hwaddr[5]));
+ pstapriv->asoc_sta_count--;
+
+ /* re-init sta_info; 20061114 */
+ _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+ _rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+ _cancel_timer_ex(&psta->addba_retry_timer);
+
+ /* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+ for (i = 0; i < 16 ; i++) {
+ struct list_head *phead, *plist;
+ struct recv_frame *prframe;
+ struct __queue *ppending_recvframe_queue;
+ struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+ preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+ _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+
+ ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+ spin_lock_bh(&ppending_recvframe_queue->lock);
+
+ phead = get_list_head(ppending_recvframe_queue);
+ plist = phead->next;
+
+ while (!list_empty(phead)) {
+ prframe = container_of(plist, struct recv_frame, list);
+
+ plist = plist->next;
+
+ list_del_init(&(prframe->list));
+
+ rtw_free_recvframe(prframe, pfree_recv_queue);
+ }
+
+ spin_unlock_bh(&ppending_recvframe_queue->lock);
+ }
+
+ if (!(psta->state & WIFI_AP_STATE))
+ rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, false);
+
+#ifdef CONFIG_88EU_AP_MODE
+
+ spin_lock_bh(&pstapriv->auth_list_lock);
+ if (!list_empty(&psta->auth_list)) {
+ list_del_init(&psta->auth_list);
+ pstapriv->auth_list_cnt--;
+ }
+ spin_unlock_bh(&pstapriv->auth_list_lock);
+
+ psta->expire_to = 0;
+
+ psta->sleepq_ac_len = 0;
+ psta->qos_info = 0;
+
+ psta->max_sp_len = 0;
+ psta->uapsd_bk = 0;
+ psta->uapsd_be = 0;
+ psta->uapsd_vi = 0;
+ psta->uapsd_vo = 0;
+ psta->has_legacy_ac = 0;
+
+ pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+ pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+ if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+ pstapriv->sta_aid[psta->aid - 1] = NULL;
+ psta->aid = 0;
+ }
+
+ psta->under_exist_checking = 0;
+
+#endif /* CONFIG_88EU_AP_MODE */
+
+ spin_lock_bh(&pfree_sta_queue->lock);
+ list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+ spin_unlock_bh(&pfree_sta_queue->lock);
+
+exit:
+
+ return _SUCCESS;
+}
+
+/* free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(struct adapter *padapter)
+{
+ struct list_head *plist, *phead;
+ s32 index;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
+
+ if (pstapriv->asoc_sta_count == 1)
+ return;
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+
+ for (index = 0; index < NUM_STA; index++) {
+ phead = &(pstapriv->sta_hash[index]);
+ plist = phead->next;
+
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info , hash_list);
+
+ plist = plist->next;
+
+ if (pbcmc_stainfo != psta)
+ rtw_free_stainfo(padapter , psta);
+ }
+ }
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+ struct list_head *plist, *phead;
+ struct sta_info *psta = NULL;
+ u32 index;
+ u8 *addr;
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+ if (hwaddr == NULL)
+ return NULL;
+
+ if (IS_MCAST(hwaddr))
+ addr = bc_addr;
+ else
+ addr = hwaddr;
+
+ index = wifi_mac_hash(addr);
+
+ spin_lock_bh(&pstapriv->sta_hash_lock);
+
+ phead = &(pstapriv->sta_hash[index]);
+ plist = phead->next;
+
+ while (phead != plist) {
+ psta = container_of(plist, struct sta_info, hash_list);
+
+ if ((!memcmp(psta->hwaddr, addr, ETH_ALEN))) {
+ /* if found the matched address */
+ break;
+ }
+ psta = NULL;
+ plist = plist->next;
+ }
+
+ spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+ return psta;
+}
+
+u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
+{
+ struct sta_info *psta;
+ u32 res = _SUCCESS;
+ unsigned char bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+ if (psta == NULL) {
+ res = _FAIL;
+ RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("rtw_alloc_stainfo fail"));
+ goto exit;
+ }
+
+ /* default broadcast & multicast use macid 1 */
+ psta->mac_id = 1;
+
+exit:
+
+ return res;
+}
+
+struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter)
+{
+ struct sta_info *psta;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+ psta = rtw_get_stainfo(pstapriv, bc_addr);
+
+ return psta;
+}
+
+u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr)
+{
+ u8 res = true;
+#ifdef CONFIG_88EU_AP_MODE
+ struct list_head *plist, *phead;
+ struct rtw_wlan_acl_node *paclnode;
+ u8 match = false;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+ struct __queue *pacl_node_q = &pacl_list->acl_node_q;
+
+ spin_lock_bh(&pacl_node_q->lock);
+ phead = get_list_head(pacl_node_q);
+ plist = phead->next;
+ while (phead != plist) {
+ paclnode = container_of(plist, struct rtw_wlan_acl_node, list);
+ plist = plist->next;
+
+ if (!memcmp(paclnode->addr, mac_addr, ETH_ALEN)) {
+ if (paclnode->valid) {
+ match = true;
+ break;
+ }
+ }
+ }
+ spin_unlock_bh(&pacl_node_q->lock);
+
+ if (pacl_list->mode == 1)/* accept unless in deny list */
+ res = (match) ? false : true;
+ else if (pacl_list->mode == 2)/* deny unless in accept list */
+ res = (match) ? true : false;
+ else
+ res = true;
+
+#endif
+
+ return res;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_wlan_util.c b/drivers/staging/r8188eu/core/rtw_wlan_util.c
new file mode 100644
index 000000000000..335d6a2c93ec
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_wlan_util.c
@@ -0,0 +1,1690 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_WLAN_UTIL_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+
+static unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+static unsigned char ARTHEROS_OUI2[] = {0x00, 0x13, 0x74};
+
+static unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18};
+static unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7};
+
+static unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96};
+static unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43};
+static unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43};
+static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
+static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
+static unsigned char EPIGRAM_OUI[] = {0x00, 0x90, 0x4c};
+
+unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+#define R2T_PHY_DELAY (0)
+
+/* define WAIT_FOR_BCN_TO_M (3000) */
+#define WAIT_FOR_BCN_TO_MIN (6000)
+#define WAIT_FOR_BCN_TO_MAX (20000)
+
+static u8 rtw_basic_rate_cck[4] = {
+ IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
+ IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_ofdm[3] = {
+ IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
+ IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_mix[7] = {
+ IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
+ IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK,
+ IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
+ IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+};
+
+int cckrates_included(unsigned char *rate, int ratelen)
+{
+ int i;
+
+ for (i = 0; i < ratelen; i++) {
+ if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
+ (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
+ return true;
+ }
+ return false;
+}
+
+int cckratesonly_included(unsigned char *rate, int ratelen)
+{
+ int i;
+
+ for (i = 0; i < ratelen; i++) {
+ if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+ (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22))
+ return false;
+ }
+
+ return true;
+}
+
+unsigned char networktype_to_raid(unsigned char network_type)
+{
+ unsigned char raid;
+
+ switch (network_type) {
+ case WIRELESS_11B:
+ raid = RATR_INX_WIRELESS_B;
+ break;
+ case WIRELESS_11A:
+ case WIRELESS_11G:
+ raid = RATR_INX_WIRELESS_G;
+ break;
+ case WIRELESS_11BG:
+ raid = RATR_INX_WIRELESS_GB;
+ break;
+ case WIRELESS_11_24N:
+ case WIRELESS_11_5N:
+ raid = RATR_INX_WIRELESS_N;
+ break;
+ case WIRELESS_11A_5N:
+ case WIRELESS_11G_24N:
+ raid = RATR_INX_WIRELESS_NG;
+ break;
+ case WIRELESS_11BG_24N:
+ raid = RATR_INX_WIRELESS_NGB;
+ break;
+ default:
+ raid = RATR_INX_WIRELESS_GB;
+ break;
+ }
+ return raid;
+}
+
+u8 judge_network_type(struct adapter *padapter, unsigned char *rate, int ratelen)
+{
+ u8 network_type = 0;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pmlmeext->cur_channel > 14) {
+ if (pmlmeinfo->HT_enable)
+ network_type = WIRELESS_11_5N;
+
+ network_type |= WIRELESS_11A;
+ } else {
+ if (pmlmeinfo->HT_enable)
+ network_type = WIRELESS_11_24N;
+
+ if ((cckratesonly_included(rate, ratelen)) == true)
+ network_type |= WIRELESS_11B;
+ else if ((cckrates_included(rate, ratelen)) == true)
+ network_type |= WIRELESS_11BG;
+ else
+ network_type |= WIRELESS_11G;
+ }
+ return network_type;
+}
+
+static unsigned char ratetbl_val_2wifirate(unsigned char rate)
+{
+ unsigned char val = 0;
+
+ switch (rate & 0x7f) {
+ case 0:
+ val = IEEE80211_CCK_RATE_1MB;
+ break;
+ case 1:
+ val = IEEE80211_CCK_RATE_2MB;
+ break;
+ case 2:
+ val = IEEE80211_CCK_RATE_5MB;
+ break;
+ case 3:
+ val = IEEE80211_CCK_RATE_11MB;
+ break;
+ case 4:
+ val = IEEE80211_OFDM_RATE_6MB;
+ break;
+ case 5:
+ val = IEEE80211_OFDM_RATE_9MB;
+ break;
+ case 6:
+ val = IEEE80211_OFDM_RATE_12MB;
+ break;
+ case 7:
+ val = IEEE80211_OFDM_RATE_18MB;
+ break;
+ case 8:
+ val = IEEE80211_OFDM_RATE_24MB;
+ break;
+ case 9:
+ val = IEEE80211_OFDM_RATE_36MB;
+ break;
+ case 10:
+ val = IEEE80211_OFDM_RATE_48MB;
+ break;
+ case 11:
+ val = IEEE80211_OFDM_RATE_54MB;
+ break;
+ }
+ return val;
+}
+
+static int is_basicrate(struct adapter *padapter, unsigned char rate)
+{
+ int i;
+ unsigned char val;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ for (i = 0; i < NumRates; i++) {
+ val = pmlmeext->basicrate[i];
+
+ if ((val != 0xff) && (val != 0xfe)) {
+ if (rate == ratetbl_val_2wifirate(val))
+ return true;
+ }
+ }
+ return false;
+}
+
+static unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset)
+{
+ int i;
+ unsigned char rate;
+ unsigned int len = 0;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+ for (i = 0; i < NumRates; i++) {
+ rate = pmlmeext->datarate[i];
+
+ switch (rate) {
+ case 0xff:
+ return len;
+ case 0xfe:
+ continue;
+ default:
+ rate = ratetbl_val_2wifirate(rate);
+
+ if (is_basicrate(padapter, rate) == true)
+ rate |= IEEE80211_BASIC_RATE_MASK;
+
+ rateset[len] = rate;
+ len++;
+ break;
+ }
+ }
+ return len;
+}
+
+void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+{
+ unsigned char supportedrates[NumRates];
+
+ memset(supportedrates, 0, NumRates);
+ *bssrate_len = ratetbl2rateset(padapter, supportedrates);
+ memcpy(pbssrate, supportedrates, *bssrate_len);
+}
+
+void UpdateBrateTbl(struct adapter *Adapter, u8 *mbrate)
+{
+ u8 i;
+ u8 rate;
+
+ /* 1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
+ for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+ rate = mbrate[i] & 0x7f;
+ switch (rate) {
+ case IEEE80211_CCK_RATE_1MB:
+ case IEEE80211_CCK_RATE_2MB:
+ case IEEE80211_CCK_RATE_5MB:
+ case IEEE80211_CCK_RATE_11MB:
+ case IEEE80211_OFDM_RATE_6MB:
+ case IEEE80211_OFDM_RATE_12MB:
+ case IEEE80211_OFDM_RATE_24MB:
+ mbrate[i] |= IEEE80211_BASIC_RATE_MASK;
+ break;
+ }
+ }
+}
+
+void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+{
+ u8 i;
+ u8 rate;
+
+ for (i = 0; i < bssratelen; i++) {
+ rate = bssrateset[i] & 0x7f;
+ switch (rate) {
+ case IEEE80211_CCK_RATE_1MB:
+ case IEEE80211_CCK_RATE_2MB:
+ case IEEE80211_CCK_RATE_5MB:
+ case IEEE80211_CCK_RATE_11MB:
+ bssrateset[i] |= IEEE80211_BASIC_RATE_MASK;
+ break;
+ }
+ }
+}
+
+void Save_DM_Func_Flag(struct adapter *padapter)
+{
+ u8 saveflag = true;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&saveflag));
+}
+
+void Restore_DM_Func_Flag(struct adapter *padapter)
+{
+ u8 saveflag = false;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&saveflag));
+}
+
+void Switch_DM_Func(struct adapter *padapter, u32 mode, u8 enable)
+{
+ if (enable)
+ rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
+ else
+ rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
+}
+
+static void Set_NETYPE0_MSR(struct adapter *padapter, u8 type)
+{
+ rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+void Set_MSR(struct adapter *padapter, u8 type)
+{
+ Set_NETYPE0_MSR(padapter, type);
+}
+
+inline u8 rtw_get_oper_ch(struct adapter *adapter)
+{
+ return adapter->mlmeextpriv.oper_channel;
+}
+
+inline void rtw_set_oper_ch(struct adapter *adapter, u8 ch)
+{
+ adapter->mlmeextpriv.oper_channel = ch;
+}
+
+inline u8 rtw_get_oper_bw(struct adapter *adapter)
+{
+ return adapter->mlmeextpriv.oper_bwmode;
+}
+
+inline void rtw_set_oper_bw(struct adapter *adapter, u8 bw)
+{
+ adapter->mlmeextpriv.oper_bwmode = bw;
+}
+
+inline u8 rtw_get_oper_choffset(struct adapter *adapter)
+{
+ return adapter->mlmeextpriv.oper_ch_offset;
+}
+
+inline void rtw_set_oper_choffset(struct adapter *adapter, u8 offset)
+{
+ adapter->mlmeextpriv.oper_ch_offset = offset;
+}
+
+void SelectChannel(struct adapter *padapter, unsigned char channel)
+{
+ /* saved channel info */
+ rtw_set_oper_ch(padapter, channel);
+ rtw_hal_set_chan(padapter, channel);
+}
+
+void SetBWMode(struct adapter *padapter, unsigned short bwmode,
+ unsigned char channel_offset)
+{
+ /* saved bw info */
+ rtw_set_oper_bw(padapter, bwmode);
+ rtw_set_oper_choffset(padapter, channel_offset);
+
+ rtw_hal_set_bwmode(padapter, (enum ht_channel_width)bwmode, channel_offset);
+}
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+ u8 center_ch;
+
+ if (padapter->bNotifyChannelChange)
+ DBG_88E("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode);
+
+ if ((bwmode == HT_CHANNEL_WIDTH_20) ||
+ (channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)) {
+ /* SelectChannel(padapter, channel); */
+ center_ch = channel;
+ } else {
+ /* switch to the proper channel */
+ if (channel_offset == HAL_PRIME_CHNL_OFFSET_LOWER) {
+ /* SelectChannel(padapter, channel + 2); */
+ center_ch = channel + 2;
+ } else {
+ /* SelectChannel(padapter, channel - 2); */
+ center_ch = channel - 2;
+ }
+ }
+
+ /* set Channel */
+ /* saved channel/bw info */
+ rtw_set_oper_ch(padapter, channel);
+ rtw_set_oper_bw(padapter, bwmode);
+ rtw_set_oper_choffset(padapter, channel_offset);
+
+ rtw_hal_set_chan(padapter, center_ch); /* set center channel */
+ SetBWMode(padapter, bwmode, channel_offset);
+}
+
+int get_bsstype(unsigned short capability)
+{
+ if (capability & BIT(0))
+ return WIFI_FW_AP_STATE;
+ else if (capability & BIT(1))
+ return WIFI_FW_ADHOC_STATE;
+ else
+ return 0;
+}
+
+__inline u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork)
+{
+ return pnetwork->MacAddress;
+}
+
+u16 get_beacon_interval(struct wlan_bssid_ex *bss)
+{
+ __le16 val;
+ memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2);
+
+ return le16_to_cpu(val);
+}
+
+int is_client_associated_to_ap(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext;
+ struct mlme_ext_info *pmlmeinfo;
+
+ if (!padapter)
+ return _FAIL;
+
+ pmlmeext = &padapter->mlmeextpriv;
+ pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE))
+ return true;
+ else
+ return _FAIL;
+}
+
+int is_client_associated_to_ibss(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+ return true;
+ else
+ return _FAIL;
+}
+
+int is_IBSS_empty(struct adapter *padapter)
+{
+ unsigned int i;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+ if (pmlmeinfo->FW_sta_info[i].status == 1)
+ return _FAIL;
+ }
+ return true;
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+ if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+ return WAIT_FOR_BCN_TO_MIN;
+ else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+ return WAIT_FOR_BCN_TO_MAX;
+ else
+ return bcn_interval << 2;
+}
+
+void CAM_empty_entry(struct adapter *Adapter, u8 ucIndex)
+{
+ rtw_hal_set_hwreg(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex));
+}
+
+void invalidate_cam_all(struct adapter *padapter)
+{
+ rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+}
+
+void write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
+{
+ unsigned int i, val, addr;
+ int j;
+ u32 cam_val[2];
+
+ addr = entry << 3;
+
+ for (j = 5; j >= 0; j--) {
+ switch (j) {
+ case 0:
+ val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
+ break;
+ case 1:
+ val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+ break;
+ default:
+ i = (j - 2) << 2;
+ val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
+ break;
+ }
+
+ cam_val[0] = val;
+ cam_val[1] = addr + (unsigned int)j;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+ }
+}
+
+void clear_cam_entry(struct adapter *padapter, u8 entry)
+{
+ unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+ unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+ write_cam(padapter, entry, 0, null_sta, null_key);
+}
+
+int allocate_fw_sta_entry(struct adapter *padapter)
+{
+ unsigned int mac_id;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
+ if (pmlmeinfo->FW_sta_info[mac_id].status == 0) {
+ pmlmeinfo->FW_sta_info[mac_id].status = 1;
+ pmlmeinfo->FW_sta_info[mac_id].retry = 0;
+ break;
+ }
+ }
+
+ return mac_id;
+}
+
+void flush_all_cam_entry(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+
+ memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+}
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+ /* struct registry_priv *pregpriv = &padapter->registrypriv; */
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pmlmepriv->qospriv.qos_option == 0) {
+ pmlmeinfo->WMM_enable = 0;
+ return _FAIL;
+ }
+
+ pmlmeinfo->WMM_enable = 1;
+ memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element));
+ return true;
+}
+
+void WMMOnAssocRsp(struct adapter *padapter)
+{
+ u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+ u8 acm_mask;
+ u16 TXOP;
+ u32 acParm, i;
+ u32 edca[4], inx[4];
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+
+ if (pmlmeinfo->WMM_enable == 0) {
+ padapter->mlmepriv.acm_mask = 0;
+ return;
+ }
+
+ acm_mask = 0;
+
+ if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+ aSifsTime = 10;
+ else
+ aSifsTime = 16;
+
+ for (i = 0; i < 4; i++) {
+ ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03;
+ ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
+
+ /* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
+ AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+
+ ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+ ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
+ TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
+
+ acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+
+ switch (ACI) {
+ case 0x0:
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+ acm_mask |= (ACM ? BIT(1) : 0);
+ edca[XMIT_BE_QUEUE] = acParm;
+ break;
+ case 0x1:
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+ edca[XMIT_BK_QUEUE] = acParm;
+ break;
+ case 0x2:
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+ acm_mask |= (ACM ? BIT(2) : 0);
+ edca[XMIT_VI_QUEUE] = acParm;
+ break;
+ case 0x3:
+ rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+ acm_mask |= (ACM ? BIT(3) : 0);
+ edca[XMIT_VO_QUEUE] = acParm;
+ break;
+ }
+
+ DBG_88E("WMM(%x): %x, %x\n", ACI, ACM, acParm);
+ }
+
+ if (padapter->registrypriv.acm_method == 1)
+ rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+ else
+ padapter->mlmepriv.acm_mask = acm_mask;
+
+ inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+ if (pregpriv->wifi_spec == 1) {
+ u32 j, tmp, change_inx = false;
+
+ /* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
+ for (i = 0; i < 4; i++) {
+ for (j = i+1; j < 4; j++) {
+ /* compare CW and AIFS */
+ if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) {
+ change_inx = true;
+ } else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+ /* compare TXOP */
+ if ((edca[j] >> 16) > (edca[i] >> 16))
+ change_inx = true;
+ }
+
+ if (change_inx) {
+ tmp = edca[i];
+ edca[i] = edca[j];
+ edca[j] = tmp;
+
+ tmp = inx[i];
+ inx[i] = inx[j];
+ inx[j] = tmp;
+
+ change_inx = false;
+ }
+ }
+ }
+ }
+
+ for (i = 0; i < 4; i++) {
+ pxmitpriv->wmm_para_seq[i] = inx[i];
+ DBG_88E("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+ }
+
+ return;
+}
+
+static void bwmode_update_check(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+ unsigned char new_bwmode;
+ unsigned char new_ch_offset;
+ struct HT_info_element *pHT_info;
+ struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+ if (!pIE)
+ return;
+
+ if (!phtpriv)
+ return;
+
+ if (pIE->Length > sizeof(struct HT_info_element))
+ return;
+
+ pHT_info = (struct HT_info_element *)pIE->data;
+
+ if ((pHT_info->infos[0] & BIT(2)) && pregistrypriv->cbw40_enable) {
+ new_bwmode = HT_CHANNEL_WIDTH_40;
+
+ switch (pHT_info->infos[0] & 0x3) {
+ case 1:
+ new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+ break;
+ case 3:
+ new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+ break;
+ default:
+ new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ break;
+ }
+ } else {
+ new_bwmode = HT_CHANNEL_WIDTH_20;
+ new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ }
+
+ if ((new_bwmode != pmlmeext->cur_bwmode) ||
+ (new_ch_offset != pmlmeext->cur_ch_offset)) {
+ pmlmeinfo->bwmode_updated = true;
+
+ pmlmeext->cur_bwmode = new_bwmode;
+ pmlmeext->cur_ch_offset = new_ch_offset;
+
+ /* update HT info also */
+ HT_info_handler(padapter, pIE);
+ } else {
+ pmlmeinfo->bwmode_updated = false;
+ }
+
+ if (pmlmeinfo->bwmode_updated) {
+ struct sta_info *psta;
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+
+ /* update ap's stainfo */
+ psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+ if (psta) {
+ struct ht_priv *phtpriv_sta = &psta->htpriv;
+
+ if (phtpriv_sta->ht_option) {
+ /* bwmode */
+ phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
+ phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+ } else {
+ phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
+ phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+ }
+ }
+ }
+}
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+ unsigned int i;
+ u8 rf_type;
+ u8 max_AMPDU_len, min_MPDU_spacing;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+ if (pIE == NULL)
+ return;
+
+ if (!phtpriv->ht_option)
+ return;
+
+ pmlmeinfo->HT_caps_enable = 1;
+
+ for (i = 0; i < (pIE->Length); i++) {
+ if (i != 2) {
+ /* Got the endian issue here. */
+ pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+ } else {
+ /* modify from fw by Thomas 2010/11/17 */
+ if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
+ max_AMPDU_len = (pIE->data[i] & 0x3);
+ else
+ max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+
+ if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
+ min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+ else
+ min_MPDU_spacing = (pIE->data[i] & 0x1c);
+
+ pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+ }
+ }
+
+ rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+ /* update the MCS rates */
+ for (i = 0; i < 16; i++) {
+ if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+ pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+ else
+ pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+ }
+ return;
+}
+
+void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+ if (pIE == NULL)
+ return;
+
+ if (!phtpriv->ht_option)
+ return;
+
+ if (pIE->Length > sizeof(struct HT_info_element))
+ return;
+
+ pmlmeinfo->HT_info_enable = 1;
+ memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->Length);
+ return;
+}
+
+void HTOnAssocRsp(struct adapter *padapter)
+{
+ unsigned char max_AMPDU_len;
+ unsigned char min_MPDU_spacing;
+ /* struct registry_priv *pregpriv = &padapter->registrypriv; */
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ DBG_88E("%s\n", __func__);
+
+ if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) {
+ pmlmeinfo->HT_enable = 1;
+ } else {
+ pmlmeinfo->HT_enable = 0;
+ return;
+ }
+
+ /* handle A-MPDU parameter field */
+ /*
+ AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+ AMPDU_para [4:2]:Min MPDU Start Spacing
+ */
+ max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+ min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+}
+
+void ERP_IE_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pIE->Length > 1)
+ return;
+
+ pmlmeinfo->ERP_enable = 1;
+ memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->Length);
+}
+
+void VCS_update(struct adapter *padapter, struct sta_info *psta)
+{
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */
+ case 0: /* off */
+ psta->rtsen = 0;
+ psta->cts2self = 0;
+ break;
+ case 1: /* on */
+ if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+ psta->rtsen = 1;
+ psta->cts2self = 0;
+ } else {
+ psta->rtsen = 0;
+ psta->cts2self = 1;
+ }
+ break;
+ case 2: /* auto */
+ default:
+ if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+ if (pregpriv->vcs_type == 1) {
+ psta->rtsen = 1;
+ psta->cts2self = 0;
+ } else {
+ psta->rtsen = 0;
+ psta->cts2self = 1;
+ }
+ } else {
+ psta->rtsen = 0;
+ psta->cts2self = 0;
+ }
+ break;
+ }
+}
+
+int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len)
+{
+ unsigned int len;
+ unsigned char *p;
+ unsigned short val16, subtype;
+ struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network);
+ /* u8 wpa_ie[255], rsn_ie[255]; */
+ u16 wpa_len = 0, rsn_len = 0;
+ u8 encryp_protocol = 0;
+ struct wlan_bssid_ex *bssid;
+ int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
+ unsigned char *pbuf;
+ u32 wpa_ielen = 0;
+ u8 *pbssid = GetAddr3Ptr(pframe);
+ u32 hidden_ssid = 0;
+ struct HT_info_element *pht_info = NULL;
+ struct ieee80211_ht_cap *pht_cap = NULL;
+ u32 bcn_channel;
+ unsigned short ht_cap_info;
+ unsigned char ht_info_infos_0;
+
+ if (is_client_associated_to_ap(Adapter) == false)
+ return true;
+
+ len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+ if (len > MAX_IE_SZ) {
+ DBG_88E("%s IE too long for survey event\n", __func__);
+ return _FAIL;
+ }
+
+ if (memcmp(cur_network->network.MacAddress, pbssid, 6)) {
+ DBG_88E("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n%pM %pM\n",
+ (pbssid), (cur_network->network.MacAddress));
+ return true;
+ }
+
+ bssid = (struct wlan_bssid_ex *)rtw_zmalloc(sizeof(struct wlan_bssid_ex));
+ if (!bssid)
+ return _FAIL;
+
+ subtype = GetFrameSubType(pframe) >> 4;
+
+ if (subtype == WIFI_BEACON)
+ bssid->Reserved[0] = 1;
+
+ bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+ /* below is to copy the information element */
+ bssid->IELength = len;
+ memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+ /* check bw and channel offset */
+ /* parsing HT_CAP_IE */
+ p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (p && len > 0) {
+ pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+ ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+ } else {
+ ht_cap_info = 0;
+ }
+ /* parsing HT_INFO_IE */
+ p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (p && len > 0) {
+ pht_info = (struct HT_info_element *)(p + 2);
+ ht_info_infos_0 = pht_info->infos[0];
+ } else {
+ ht_info_infos_0 = 0;
+ }
+ if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
+ ((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) {
+ DBG_88E("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+ ht_cap_info, ht_info_infos_0);
+ DBG_88E("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+ cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
+ DBG_88E("%s bw mode change, disconnect\n", __func__);
+ /* bcn_info_update */
+ cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+ cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+ /* to do : need to check that whether modify related register of BB or not */
+ /* goto _mismatch; */
+ }
+
+ /* Checking for channel */
+ p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (p) {
+ bcn_channel = *(p + 2);
+ } else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+ p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (pht_info) {
+ bcn_channel = pht_info->primary_channel;
+ } else { /* we don't find channel IE, so don't check it */
+ DBG_88E("Oops: %s we don't find channel IE, so don't check it\n", __func__);
+ bcn_channel = Adapter->mlmeextpriv.cur_channel;
+ }
+ }
+ if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
+ DBG_88E("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
+ bcn_channel, Adapter->mlmeextpriv.cur_channel);
+ goto _mismatch;
+ }
+
+ /* checking SSID */
+ p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+ if (p == NULL) {
+ DBG_88E("%s marc: cannot find SSID for survey event\n", __func__);
+ hidden_ssid = true;
+ } else {
+ hidden_ssid = false;
+ }
+
+ if ((NULL != p) && (false == hidden_ssid && (*(p + 1)))) {
+ memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+ bssid->Ssid.SsidLength = *(p + 1);
+ } else {
+ bssid->Ssid.SsidLength = 0;
+ bssid->Ssid.Ssid[0] = '\0';
+ }
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d "
+ "cur_network->network.Ssid.Ssid:%s len:%d\n", __func__, bssid->Ssid.Ssid,
+ bssid->Ssid.SsidLength, cur_network->network.Ssid.Ssid,
+ cur_network->network.Ssid.SsidLength));
+
+ if (memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) ||
+ bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) {
+ if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0) { /* not hidden ssid */
+ DBG_88E("%s(), SSID is not match return FAIL\n", __func__);
+ goto _mismatch;
+ }
+ }
+
+ /* check encryption info */
+ val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+ if (val16 & BIT(4))
+ bssid->Privacy = 1;
+ else
+ bssid->Privacy = 0;
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
+ __func__, cur_network->network.Privacy, bssid->Privacy));
+ if (cur_network->network.Privacy != bssid->Privacy) {
+ DBG_88E("%s(), privacy is not match return FAIL\n", __func__);
+ goto _mismatch;
+ }
+
+ rtw_get_sec_ie(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL, &wpa_len);
+
+ if (rsn_len > 0) {
+ encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+ } else if (wpa_len > 0) {
+ encryp_protocol = ENCRYP_PROTOCOL_WPA;
+ } else {
+ if (bssid->Privacy)
+ encryp_protocol = ENCRYP_PROTOCOL_WEP;
+ }
+
+ if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) {
+ DBG_88E("%s(): enctyp is not match , return FAIL\n", __func__);
+ goto _mismatch;
+ }
+
+ if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+ pbuf = rtw_get_wpa_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+ if (pbuf && (wpa_ielen > 0)) {
+ if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
+ pairwise_cipher, group_cipher, is_8021x));
+ }
+ } else {
+ pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+
+ if (pbuf && (wpa_ielen > 0)) {
+ if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+ ("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
+ __func__, pairwise_cipher, group_cipher, is_8021x));
+ }
+ }
+ }
+
+ RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+ ("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher));
+ if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
+ DBG_88E("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match , return FAIL\n", __func__,
+ pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
+ group_cipher, cur_network->BcnInfo.group_cipher);
+ goto _mismatch;
+ }
+
+ if (is_8021x != cur_network->BcnInfo.is_8021x) {
+ DBG_88E("%s authentication is not match , return FAIL\n", __func__);
+ goto _mismatch;
+ }
+ }
+
+ kfree(bssid);
+
+ return _SUCCESS;
+
+_mismatch:
+ kfree(bssid);
+
+ return _FAIL;
+}
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+ unsigned int i;
+ unsigned int len;
+ struct ndis_802_11_var_ie *pIE;
+
+ len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+ for (i = 0; i < len;) {
+ pIE = (struct ndis_802_11_var_ie *)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+
+ switch (pIE->ElementID) {
+ case _HT_EXTRA_INFO_IE_: /* HT info */
+ /* HT_info_handler(padapter, pIE); */
+ bwmode_update_check(padapter, pIE);
+ break;
+ case _ERPINFO_IE_:
+ ERP_IE_handler(padapter, pIE);
+ VCS_update(padapter, psta);
+ break;
+ default:
+ break;
+ }
+
+ i += (pIE->Length + 2);
+ }
+}
+
+unsigned int is_ap_in_tkip(struct adapter *padapter)
+{
+ u32 i;
+ struct ndis_802_11_var_ie *pIE;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+
+ if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4)))
+ return true;
+ break;
+ case _RSN_IE_2_:
+ if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4))
+ return true;
+ default:
+ break;
+ }
+
+ i += (pIE->Length + 2);
+ }
+ return false;
+ } else {
+ return false;
+ }
+}
+
+unsigned int should_forbid_n_rate(struct adapter *padapter)
+{
+ u32 i;
+ struct ndis_802_11_var_ie *pIE;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct wlan_bssid_ex *cur_network = &pmlmepriv->cur_network.network;
+
+ if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < cur_network->IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(cur_network->IEs + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if (!memcmp(pIE->data, RTW_WPA_OUI, 4) &&
+ ((!memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP, 4)) ||
+ (!memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP, 4))))
+ return false;
+ break;
+ case _RSN_IE_2_:
+ if ((!memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP, 4)) ||
+ (!memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP, 4)))
+ return false;
+ default:
+ break;
+ }
+
+ i += (pIE->Length + 2);
+ }
+
+ return true;
+ } else {
+ return false;
+ }
+}
+
+unsigned int is_ap_in_wep(struct adapter *padapter)
+{
+ u32 i;
+ struct ndis_802_11_var_ie *pIE;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+
+ if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+ pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if (!memcmp(pIE->data, RTW_WPA_OUI, 4))
+ return false;
+ break;
+ case _RSN_IE_2_:
+ return false;
+ default:
+ break;
+ }
+ i += (pIE->Length + 2);
+ }
+ return true;
+ } else {
+ return false;
+ }
+}
+
+int wifirate2_ratetbl_inx(unsigned char rate)
+{
+ int inx = 0;
+ rate = rate & 0x7f;
+
+ switch (rate) {
+ case 54*2:
+ inx = 11;
+ break;
+ case 48*2:
+ inx = 10;
+ break;
+ case 36*2:
+ inx = 9;
+ break;
+ case 24*2:
+ inx = 8;
+ break;
+ case 18*2:
+ inx = 7;
+ break;
+ case 12*2:
+ inx = 6;
+ break;
+ case 9*2:
+ inx = 5;
+ break;
+ case 6*2:
+ inx = 4;
+ break;
+ case 11*2:
+ inx = 3;
+ break;
+ case 11:
+ inx = 2;
+ break;
+ case 2*2:
+ inx = 1;
+ break;
+ case 1*2:
+ inx = 0;
+ break;
+ }
+ return inx;
+}
+
+unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+ unsigned int i, num_of_rate;
+ unsigned int mask = 0;
+
+ num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+ for (i = 0; i < num_of_rate; i++) {
+ if ((*(ptn + i)) & 0x80)
+ mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+ }
+ return mask;
+}
+
+unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+ unsigned int i, num_of_rate;
+ unsigned int mask = 0;
+
+ num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+ for (i = 0; i < num_of_rate; i++)
+ mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+ return mask;
+}
+
+unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps)
+{
+ unsigned int mask = 0;
+
+ mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+
+ return mask;
+}
+
+int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps)
+{
+ unsigned char bit_offset;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (!(pmlmeinfo->HT_enable))
+ return _FAIL;
+
+ if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK))
+ return _FAIL;
+
+ bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40) ? 6 : 5;
+
+ if (__le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & (0x1 << bit_offset))
+ return _SUCCESS;
+ else
+ return _FAIL;
+}
+
+unsigned char get_highest_rate_idx(u32 mask)
+{
+ int i;
+ unsigned char rate_idx = 0;
+
+ for (i = 27; i >= 0; i--) {
+ if (mask & BIT(i)) {
+ rate_idx = i;
+ break;
+ }
+ }
+ return rate_idx;
+}
+
+void Update_RA_Entry(struct adapter *padapter, u32 mac_id)
+{
+ rtw_hal_update_ra_mask(padapter, mac_id, 0);
+}
+
+static void enable_rate_adaptive(struct adapter *padapter, u32 mac_id)
+{
+ Update_RA_Entry(padapter, mac_id);
+}
+
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta)
+{
+ /* rate adaptive */
+ enable_rate_adaptive(padapter, psta->mac_id);
+}
+
+/* Update RRSR and Rate for USERATE */
+void update_tx_basic_rate(struct adapter *padapter, u8 wirelessmode)
+{
+ unsigned char supported_rates[NDIS_802_11_LENGTH_RATES_EX];
+#ifdef CONFIG_88EU_P2P
+ struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+ /* Added by Albert 2011/03/22 */
+ /* In the P2P mode, the driver should not support the b mode. */
+ /* So, the Tx packet shouldn't use the CCK rate */
+ if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+ return;
+#endif /* CONFIG_88EU_P2P */
+ memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+ if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B))
+ memcpy(supported_rates, rtw_basic_rate_cck, 4);
+ else if (wirelessmode & WIRELESS_11B)
+ memcpy(supported_rates, rtw_basic_rate_mix, 7);
+ else
+ memcpy(supported_rates, rtw_basic_rate_ofdm, 3);
+
+ if (wirelessmode & WIRELESS_11B)
+ update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+ else
+ update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+
+ rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, supported_rates);
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+ unsigned int i;
+ struct ndis_802_11_var_ie *pIE;
+ u8 epigram_vendor_flag;
+ u8 ralink_vendor_flag;
+ epigram_vendor_flag = 0;
+ ralink_vendor_flag = 0;
+
+ for (i = sizeof(struct ndis_802_11_fixed_ie); i < len;) {
+ pIE = (struct ndis_802_11_var_ie *)(pframe + i);
+
+ switch (pIE->ElementID) {
+ case _VENDOR_SPECIFIC_IE_:
+ if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) ||
+ (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+ DBG_88E("link to Artheros AP\n");
+ return HT_IOT_PEER_ATHEROS;
+ } else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
+ (!memcmp(pIE->data, BROADCOM_OUI2, 3)) ||
+ (!memcmp(pIE->data, BROADCOM_OUI2, 3))) {
+ DBG_88E("link to Broadcom AP\n");
+ return HT_IOT_PEER_BROADCOM;
+ } else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+ DBG_88E("link to Marvell AP\n");
+ return HT_IOT_PEER_MARVELL;
+ } else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
+ if (!ralink_vendor_flag) {
+ ralink_vendor_flag = 1;
+ } else {
+ DBG_88E("link to Ralink AP\n");
+ return HT_IOT_PEER_RALINK;
+ }
+ } else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+ DBG_88E("link to Cisco AP\n");
+ return HT_IOT_PEER_CISCO;
+ } else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+ DBG_88E("link to Realtek 96B\n");
+ return HT_IOT_PEER_REALTEK;
+ } else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+ DBG_88E("link to Airgo Cap\n");
+ return HT_IOT_PEER_AIRGO;
+ } else if (!memcmp(pIE->data, EPIGRAM_OUI, 3)) {
+ epigram_vendor_flag = 1;
+ if (ralink_vendor_flag) {
+ DBG_88E("link to Tenda W311R AP\n");
+ return HT_IOT_PEER_TENDA;
+ } else {
+ DBG_88E("Capture EPIGRAM_OUI\n");
+ }
+ } else {
+ break;
+ }
+
+ default:
+ break;
+ }
+ i += (pIE->Length + 2);
+ }
+
+ if (ralink_vendor_flag && !epigram_vendor_flag) {
+ DBG_88E("link to Ralink AP\n");
+ return HT_IOT_PEER_RALINK;
+ } else if (ralink_vendor_flag && epigram_vendor_flag) {
+ DBG_88E("link to Tenda W311R AP\n");
+ return HT_IOT_PEER_TENDA;
+ } else {
+ DBG_88E("link to new AP\n");
+ return HT_IOT_PEER_UNKNOWN;
+ }
+}
+
+void update_IOT_info(struct adapter *padapter)
+{
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ switch (pmlmeinfo->assoc_AP_vendor) {
+ case HT_IOT_PEER_MARVELL:
+ pmlmeinfo->turboMode_cts2self = 1;
+ pmlmeinfo->turboMode_rtsen = 0;
+ break;
+ case HT_IOT_PEER_RALINK:
+ pmlmeinfo->turboMode_cts2self = 0;
+ pmlmeinfo->turboMode_rtsen = 1;
+ /* disable high power */
+ Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+ break;
+ case HT_IOT_PEER_REALTEK:
+ /* rtw_write16(padapter, 0x4cc, 0xffff); */
+ /* rtw_write16(padapter, 0x546, 0x01c0); */
+ /* disable high power */
+ Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+ break;
+ default:
+ pmlmeinfo->turboMode_cts2self = 0;
+ pmlmeinfo->turboMode_rtsen = 1;
+ break;
+ }
+}
+
+void update_capinfo(struct adapter *Adapter, u16 updateCap)
+{
+ struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ bool ShortPreamble;
+
+ /* Check preamble mode, 2005.01.06, by rcnjko. */
+ /* Mark to update preamble value forever, 2008.03.18 by lanhsin */
+
+ if (updateCap & cShortPreamble) { /* Short Preamble */
+ if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /* PREAMBLE_LONG or PREAMBLE_AUTO */
+ ShortPreamble = true;
+ pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
+ rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+ }
+ } else { /* Long Preamble */
+ if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) { /* PREAMBLE_SHORT or PREAMBLE_AUTO */
+ ShortPreamble = false;
+ pmlmeinfo->preamble_mode = PREAMBLE_LONG;
+ rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+ }
+ }
+
+ if (updateCap & cIBSS) {
+ /* Filen: See 802.11-2007 p.91 */
+ pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+ } else { /* Filen: See 802.11-2007 p.90 */
+ if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11_24N)) {
+ if (updateCap & cShortSlotTime) { /* Short Slot Time */
+ if (pmlmeinfo->slotTime != SHORT_SLOT_TIME)
+ pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+ } else { /* Long Slot Time */
+ if (pmlmeinfo->slotTime != NON_SHORT_SLOT_TIME)
+ pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+ }
+ } else if (pmlmeext->cur_wireless_mode & (WIRELESS_11A | WIRELESS_11_5N)) {
+ pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+ } else {
+ /* B Mode */
+ pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+ }
+ }
+
+ rtw_hal_set_hwreg(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime);
+}
+
+void update_wireless_mode(struct adapter *padapter)
+{
+ int ratelen, network_type = 0;
+ u32 SIFS_Timer;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+ struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+ unsigned char *rate = cur_network->SupportedRates;
+
+ ratelen = rtw_get_rateset_len(cur_network->SupportedRates);
+
+ if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+ pmlmeinfo->HT_enable = 1;
+
+ if (pmlmeext->cur_channel > 14) {
+ if (pmlmeinfo->HT_enable)
+ network_type = WIRELESS_11_5N;
+
+ network_type |= WIRELESS_11A;
+ } else {
+ if (pmlmeinfo->HT_enable)
+ network_type = WIRELESS_11_24N;
+
+ if ((cckratesonly_included(rate, ratelen)) == true)
+ network_type |= WIRELESS_11B;
+ else if ((cckrates_included(rate, ratelen)) == true)
+ network_type |= WIRELESS_11BG;
+ else
+ network_type |= WIRELESS_11G;
+ }
+
+ pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+
+ SIFS_Timer = 0x0a0a0808;/* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
+ /* change this value if having IOT issues. */
+
+ padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS, (u8 *)&SIFS_Timer);
+
+ if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+ update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+ else
+ update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void update_bmc_sta_support_rate(struct adapter *padapter, u32 mac_id)
+{
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
+ /* Only B, B/G, and B/G/N AP could use CCK rate */
+ memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_cck, 4);
+ } else {
+ memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_ofdm, 3);
+ }
+}
+
+int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+{
+ unsigned int ie_len;
+ struct ndis_802_11_var_ie *pIE;
+ int supportRateNum = 0;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+ if (pIE == NULL)
+ return _FAIL;
+
+ memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
+ supportRateNum = ie_len;
+
+ pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+ if (pIE)
+ memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+
+ return _SUCCESS;
+}
+
+void process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+ struct sta_info *psta;
+ u16 tid;
+ u16 param;
+ struct recv_reorder_ctrl *preorder_ctrl;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct ADDBA_request *preq = (struct ADDBA_request *)paddba_req;
+ struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ psta = rtw_get_stainfo(pstapriv, addr);
+
+ if (psta) {
+ param = le16_to_cpu(preq->BA_para_set);
+ tid = (param>>2)&0x0f;
+ preorder_ctrl = &psta->recvreorder_ctrl[tid];
+ preorder_ctrl->indicate_seq = 0xffff;
+ preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq) ? true : false;
+ }
+}
+
+void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+ u8 *pIE;
+ __le32 *pbuf;
+
+ pIE = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+ pbuf = (__le32 *)pIE;
+
+ pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1));
+
+ pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
+
+ pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
+}
+
+void correct_TSF(struct adapter *padapter, struct mlme_ext_priv *pmlmeext)
+{
+ rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, NULL);
+}
+
+void beacon_timing_control(struct adapter *padapter)
+{
+ rtw_hal_bcn_related_reg_setting(padapter);
+}
+
+static struct adapter *pbuddy_padapter;
+
+int rtw_handle_dualmac(struct adapter *adapter, bool init)
+{
+ int status = _SUCCESS;
+
+ if (init) {
+ if (pbuddy_padapter == NULL) {
+ pbuddy_padapter = adapter;
+ DBG_88E("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n", __func__);
+ } else {
+ adapter->pbuddy_adapter = pbuddy_padapter;
+ pbuddy_padapter->pbuddy_adapter = adapter;
+ /* clear global value */
+ pbuddy_padapter = NULL;
+ DBG_88E("%s(): pbuddy_padapter exist, Exchange Information\n", __func__);
+ }
+ } else {
+ pbuddy_padapter = NULL;
+ }
+ return status;
+}
diff --git a/drivers/staging/r8188eu/core/rtw_xmit.c b/drivers/staging/r8188eu/core/rtw_xmit.c
new file mode 100644
index 000000000000..867cd3bbd135
--- /dev/null
+++ b/drivers/staging/r8188eu/core/rtw_xmit.c
@@ -0,0 +1,2370 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_XMIT_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <osdep_intf.h>
+#include <ip.h>
+#include <usb_ops.h>
+#include <usb_osintf.h>
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+
+ INIT_LIST_HEAD(&ptxservq->tx_pending);
+ _rtw_init_queue(&ptxservq->sta_pending);
+ ptxservq->qcnt = 0;
+
+}
+
+void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{
+
+ memset((unsigned char *)psta_xmitpriv, 0, sizeof (struct sta_xmit_priv));
+ spin_lock_init(&psta_xmitpriv->lock);
+ _init_txservq(&psta_xmitpriv->be_q);
+ _init_txservq(&psta_xmitpriv->bk_q);
+ _init_txservq(&psta_xmitpriv->vi_q);
+ _init_txservq(&psta_xmitpriv->vo_q);
+ INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
+ INIT_LIST_HEAD(&psta_xmitpriv->apsd);
+
+}
+
+s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter)
+{
+ int i;
+ struct xmit_buf *pxmitbuf;
+ struct xmit_frame *pxframe;
+ int res = _SUCCESS;
+ u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+ u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+
+ /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+
+ spin_lock_init(&pxmitpriv->lock);
+ sema_init(&pxmitpriv->xmit_sema, 0);
+ sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
+
+ /*
+ Please insert all the queue initializaiton using _rtw_init_queue below
+ */
+
+ pxmitpriv->adapter = padapter;
+
+ _rtw_init_queue(&pxmitpriv->be_pending);
+ _rtw_init_queue(&pxmitpriv->bk_pending);
+ _rtw_init_queue(&pxmitpriv->vi_pending);
+ _rtw_init_queue(&pxmitpriv->vo_pending);
+ _rtw_init_queue(&pxmitpriv->bm_pending);
+
+ _rtw_init_queue(&pxmitpriv->free_xmit_queue);
+
+ /*
+ Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
+ and initialize free_xmit_frame below.
+ Please also apply free_txobj to link_up all the xmit_frames...
+ */
+
+ pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+ if (pxmitpriv->pallocated_frame_buf == NULL) {
+ pxmitpriv->pxmit_frame_buf = NULL;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
+ res = _FAIL;
+ goto exit;
+ }
+ pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((size_t)(pxmitpriv->pallocated_frame_buf), 4);
+ /* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
+ /* ((size_t) (pxmitpriv->pallocated_frame_buf) &3); */
+
+ pxframe = (struct xmit_frame *)pxmitpriv->pxmit_frame_buf;
+
+ for (i = 0; i < NR_XMITFRAME; i++) {
+ INIT_LIST_HEAD(&(pxframe->list));
+
+ pxframe->padapter = padapter;
+ pxframe->frame_tag = NULL_FRAMETAG;
+
+ pxframe->pkt = NULL;
+
+ pxframe->buf_addr = NULL;
+ pxframe->pxmitbuf = NULL;
+
+ list_add_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue));
+
+ pxframe++;
+ }
+
+ pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+
+ pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+
+ /* init xmit_buf */
+ _rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
+ _rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
+
+ pxmitpriv->pallocated_xmitbuf = rtw_zvmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+ if (pxmitpriv->pallocated_xmitbuf == NULL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_buf fail!\n"));
+ res = _FAIL;
+ goto exit;
+ }
+
+ pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((size_t)(pxmitpriv->pallocated_xmitbuf), 4);
+ /* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
+ /* ((size_t) (pxmitpriv->pallocated_xmitbuf) &3); */
+
+ pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+ for (i = 0; i < NR_XMITBUFF; i++) {
+ INIT_LIST_HEAD(&pxmitbuf->list);
+
+ pxmitbuf->priv_data = NULL;
+ pxmitbuf->padapter = padapter;
+ pxmitbuf->ext_tag = false;
+
+ /* Tx buf allocation may fail sometimes, so sleep and retry. */
+ res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+ if (res == _FAIL) {
+ rtw_msleep_os(10);
+ res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+ if (res == _FAIL) {
+ goto exit;
+ }
+ }
+
+ pxmitbuf->flags = XMIT_VO_QUEUE;
+
+ list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue));
+ pxmitbuf++;
+ }
+
+ pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+
+ /* Init xmit extension buff */
+ _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+ pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+
+ if (pxmitpriv->pallocated_xmit_extbuf == NULL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n"));
+ res = _FAIL;
+ goto exit;
+ }
+
+ pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((size_t)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+ pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+ for (i = 0; i < num_xmit_extbuf; i++) {
+ INIT_LIST_HEAD(&pxmitbuf->list);
+
+ pxmitbuf->priv_data = NULL;
+ pxmitbuf->padapter = padapter;
+ pxmitbuf->ext_tag = true;
+
+ res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, max_xmit_extbuf_size + XMITBUF_ALIGN_SZ);
+ if (res == _FAIL) {
+ res = _FAIL;
+ goto exit;
+ }
+
+ list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+ pxmitbuf++;
+ }
+
+ pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+ rtw_alloc_hwxmits(padapter);
+ rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+
+ for (i = 0; i < 4; i++)
+ pxmitpriv->wmm_para_seq[i] = i;
+
+ pxmitpriv->txirp_cnt = 1;
+
+ sema_init(&(pxmitpriv->tx_retevt), 0);
+
+ /* per AC pending irp */
+ pxmitpriv->beq_cnt = 0;
+ pxmitpriv->bkq_cnt = 0;
+ pxmitpriv->viq_cnt = 0;
+ pxmitpriv->voq_cnt = 0;
+
+ pxmitpriv->ack_tx = false;
+ _rtw_mutex_init(&pxmitpriv->ack_tx_mutex);
+ rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
+
+ rtw_hal_init_xmit_priv(padapter);
+
+exit:
+
+ return res;
+}
+
+static void rtw_mfree_xmit_priv_lock (struct xmit_priv *pxmitpriv)
+{
+ _rtw_spinlock_free(&pxmitpriv->lock);
+
+ _rtw_spinlock_free(&pxmitpriv->be_pending.lock);
+ _rtw_spinlock_free(&pxmitpriv->bk_pending.lock);
+ _rtw_spinlock_free(&pxmitpriv->vi_pending.lock);
+ _rtw_spinlock_free(&pxmitpriv->vo_pending.lock);
+ _rtw_spinlock_free(&pxmitpriv->bm_pending.lock);
+
+ _rtw_spinlock_free(&pxmitpriv->free_xmit_queue.lock);
+ _rtw_spinlock_free(&pxmitpriv->free_xmitbuf_queue.lock);
+ _rtw_spinlock_free(&pxmitpriv->pending_xmitbuf_queue.lock);
+}
+
+void _rtw_free_xmit_priv (struct xmit_priv *pxmitpriv)
+{
+ int i;
+ struct adapter *padapter = pxmitpriv->adapter;
+ struct xmit_frame *pxmitframe = (struct xmit_frame *)pxmitpriv->pxmit_frame_buf;
+ struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+ u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+ u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+
+ rtw_hal_free_xmit_priv(padapter);
+
+ rtw_mfree_xmit_priv_lock(pxmitpriv);
+
+ if (pxmitpriv->pxmit_frame_buf == NULL)
+ return;
+
+ for (i = 0; i < NR_XMITFRAME; i++) {
+ rtw_os_xmit_complete(padapter, pxmitframe);
+
+ pxmitframe++;
+ }
+
+ for (i = 0; i < NR_XMITBUFF; i++) {
+ rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+ pxmitbuf++;
+ }
+
+ if (pxmitpriv->pallocated_frame_buf)
+ rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+ if (pxmitpriv->pallocated_xmitbuf)
+ rtw_vmfree(pxmitpriv->pallocated_xmitbuf, NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+ /* free xmit extension buff */
+ _rtw_spinlock_free(&pxmitpriv->free_xmit_extbuf_queue.lock);
+
+ pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+ for (i = 0; i < num_xmit_extbuf; i++) {
+ rtw_os_xmit_resource_free(padapter, pxmitbuf, (max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+ pxmitbuf++;
+ }
+
+ if (pxmitpriv->pallocated_xmit_extbuf) {
+ rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+ }
+
+ rtw_free_hwxmits(padapter);
+
+ _rtw_mutex_free(&pxmitpriv->ack_tx_mutex);
+}
+
+static void update_attrib_vcs_info(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ u32 sz;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ struct sta_info *psta = pattrib->psta;
+ struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+ struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+ if (pattrib->nr_frags != 1)
+ sz = padapter->xmitpriv.frag_len;
+ else /* no frag */
+ sz = pattrib->last_txcmdsz;
+
+ /* (1) RTS_Threshold is compared to the MPDU, not MSDU. */
+ /* (2) If there are more than one frag in this MSDU, only the first frag uses protection frame. */
+ /* Other fragments are protected by previous fragment. */
+ /* So we only need to check the length of first fragment. */
+ if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N || padapter->registrypriv.wifi_spec) {
+ if (sz > padapter->registrypriv.rts_thresh) {
+ pattrib->vcs_mode = RTS_CTS;
+ } else {
+ if (psta->rtsen)
+ pattrib->vcs_mode = RTS_CTS;
+ else if (psta->cts2self)
+ pattrib->vcs_mode = CTS_TO_SELF;
+ else
+ pattrib->vcs_mode = NONE_VCS;
+ }
+ } else {
+ while (true) {
+ /* IOT action */
+ if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && pattrib->ampdu_en &&
+ (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+ pattrib->vcs_mode = CTS_TO_SELF;
+ break;
+ }
+
+ /* check ERP protection */
+ if (psta->rtsen || psta->cts2self) {
+ if (psta->rtsen)
+ pattrib->vcs_mode = RTS_CTS;
+ else if (psta->cts2self)
+ pattrib->vcs_mode = CTS_TO_SELF;
+
+ break;
+ }
+
+ /* check HT op mode */
+ if (pattrib->ht_en) {
+ u8 htopmode = pmlmeinfo->HT_protection;
+ if ((pmlmeext->cur_bwmode && (htopmode == 2 || htopmode == 3)) ||
+ (!pmlmeext->cur_bwmode && htopmode == 3)) {
+ pattrib->vcs_mode = RTS_CTS;
+ break;
+ }
+ }
+
+ /* check rts */
+ if (sz > padapter->registrypriv.rts_thresh) {
+ pattrib->vcs_mode = RTS_CTS;
+ break;
+ }
+
+ /* to do list: check MIMO power save condition. */
+
+ /* check AMPDU aggregation for TXOP */
+ if (pattrib->ampdu_en) {
+ pattrib->vcs_mode = RTS_CTS;
+ break;
+ }
+
+ pattrib->vcs_mode = NONE_VCS;
+ break;
+ }
+ }
+}
+
+static void update_attrib_phy_info(struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+ /*if (psta->rtsen)
+ pattrib->vcs_mode = RTS_CTS;
+ else if (psta->cts2self)
+ pattrib->vcs_mode = CTS_TO_SELF;
+ else
+ pattrib->vcs_mode = NONE_VCS;*/
+
+ pattrib->mdata = 0;
+ pattrib->eosp = 0;
+ pattrib->triggered = 0;
+
+ /* qos_en, ht_en, init rate, , bw, ch_offset, sgi */
+ pattrib->qos_en = psta->qos_option;
+
+ pattrib->raid = psta->raid;
+ pattrib->ht_en = psta->htpriv.ht_option;
+ pattrib->bwmode = psta->htpriv.bwmode;
+ pattrib->ch_offset = psta->htpriv.ch_offset;
+ pattrib->sgi = psta->htpriv.sgi;
+ pattrib->ampdu_en = false;
+ pattrib->retry_ctrl = false;
+}
+
+u8 qos_acm(u8 acm_mask, u8 priority)
+{
+ u8 change_priority = priority;
+
+ switch (priority) {
+ case 0:
+ case 3:
+ if (acm_mask & BIT(1))
+ change_priority = 1;
+ break;
+ case 1:
+ case 2:
+ break;
+ case 4:
+ case 5:
+ if (acm_mask & BIT(2))
+ change_priority = 0;
+ break;
+ case 6:
+ case 7:
+ if (acm_mask & BIT(3))
+ change_priority = 5;
+ break;
+ default:
+ DBG_88E("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
+ break;
+ }
+
+ return change_priority;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+ struct ethhdr etherhdr;
+ struct iphdr ip_hdr;
+ s32 user_prio = 0;
+
+ _rtw_open_pktfile(ppktfile->pkt, ppktfile);
+ _rtw_pktfile_read(ppktfile, (unsigned char *)ðerhdr, ETH_HLEN);
+
+ /* get user_prio from IP hdr */
+ if (pattrib->ether_type == 0x0800) {
+ _rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+/* user_prio = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
+ user_prio = ip_hdr.tos >> 5;
+ } else if (pattrib->ether_type == 0x888e) {
+ /* "When priority processing of data frames is supported, */
+ /* a STA's SME should send EAPOL-Key frames at the highest priority." */
+ user_prio = 7;
+ }
+
+ pattrib->priority = user_prio;
+ pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+ pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct pkt_attrib *pattrib)
+{
+ struct pkt_file pktfile;
+ struct sta_info *psta = NULL;
+ struct ethhdr etherhdr;
+
+ int bmcast;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+ int res = _SUCCESS;
+
+
+
+ _rtw_open_pktfile(pkt, &pktfile);
+ _rtw_pktfile_read(&pktfile, (u8 *)ðerhdr, ETH_HLEN);
+
+ pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+ memcpy(pattrib->dst, ðerhdr.h_dest, ETH_ALEN);
+ memcpy(pattrib->src, ðerhdr.h_source, ETH_ALEN);
+
+ pattrib->pctrl = 0;
+
+ if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+ memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+ memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+ } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+ memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+ memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+ } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+ memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+ }
+
+ pattrib->pktlen = pktfile.pkt_len;
+
+ if (ETH_P_IP == pattrib->ether_type) {
+ /* The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */
+ /* to prevent DHCP protocol fail */
+ u8 tmp[24];
+ _rtw_pktfile_read(&pktfile, &tmp[0], 24);
+ pattrib->dhcp_pkt = 0;
+ if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
+ if (ETH_P_IP == pattrib->ether_type) {/* IP header */
+ if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+ ((tmp[21] == 67) && (tmp[23] == 68))) {
+ /* 68 : UDP BOOTP client */
+ /* 67 : UDP BOOTP server */
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("====================== update_attrib: get DHCP Packet\n"));
+ /* Use low rate to send DHCP packet. */
+ pattrib->dhcp_pkt = 1;
+ }
+ }
+ }
+ } else if (0x888e == pattrib->ether_type) {
+ DBG_88E_LEVEL(_drv_info_, "send eapol packet\n");
+ }
+
+ if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+ rtw_set_scan_deny(padapter, 3000);
+
+ /* If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
+ if ((pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+ rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+
+ bmcast = IS_MCAST(pattrib->ra);
+
+ /* get sta_info */
+ if (bmcast) {
+ psta = rtw_get_bcmc_stainfo(padapter);
+ } else {
+ psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+ if (psta == NULL) { /* if we cannot get psta => drrp the pkt */
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra: %pM\n", (pattrib->ra)));
+ res = _FAIL;
+ goto exit;
+ } else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) {
+ res = _FAIL;
+ goto exit;
+ }
+ }
+
+ if (psta) {
+ pattrib->mac_id = psta->mac_id;
+ /* DBG_88E("%s ==> mac_id(%d)\n", __func__, pattrib->mac_id); */
+ pattrib->psta = psta;
+ } else {
+ /* if we cannot get psta => drop the pkt */
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:%pM\n", (pattrib->ra)));
+ res = _FAIL;
+ goto exit;
+ }
+
+ pattrib->ack_policy = 0;
+ /* get ether_hdr_len */
+ pattrib->pkt_hdrlen = ETH_HLEN;/* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */
+
+ pattrib->hdrlen = WLAN_HDR_A3_LEN;
+ pattrib->subtype = WIFI_DATA_TYPE;
+ pattrib->priority = 0;
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) {
+ if (psta->qos_option)
+ set_qos(&pktfile, pattrib);
+ } else {
+ if (pqospriv->qos_option) {
+ set_qos(&pktfile, pattrib);
+
+ if (pmlmepriv->acm_mask != 0)
+ pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
+ }
+ }
+
+ if (psta->ieee8021x_blocked) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\n psta->ieee8021x_blocked == true\n"));
+
+ pattrib->encrypt = 0;
+
+ if ((pattrib->ether_type != 0x888e) && !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\npsta->ieee8021x_blocked == true, pattrib->ether_type(%.4x) != 0x888e\n", pattrib->ether_type));
+ res = _FAIL;
+ goto exit;
+ }
+ } else {
+ GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+ switch (psecuritypriv->dot11AuthAlgrthm) {
+ case dot11AuthAlgrthm_Open:
+ case dot11AuthAlgrthm_Shared:
+ case dot11AuthAlgrthm_Auto:
+ pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+ break;
+ case dot11AuthAlgrthm_8021X:
+ if (bmcast)
+ pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
+ else
+ pattrib->key_idx = 0;
+ break;
+ default:
+ pattrib->key_idx = 0;
+ break;
+ }
+ }
+
+ switch (pattrib->encrypt) {
+ case _WEP40_:
+ case _WEP104_:
+ pattrib->iv_len = 4;
+ pattrib->icv_len = 4;
+ break;
+ case _TKIP_:
+ pattrib->iv_len = 8;
+ pattrib->icv_len = 4;
+
+ if (padapter->securitypriv.busetkipkey == _FAIL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+ ("\npadapter->securitypriv.busetkipkey(%d) == _FAIL drop packet\n",
+ padapter->securitypriv.busetkipkey));
+ res = _FAIL;
+ goto exit;
+ }
+ break;
+ case _AES_:
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("pattrib->encrypt=%d (_AES_)\n", pattrib->encrypt));
+ pattrib->iv_len = 8;
+ pattrib->icv_len = 8;
+ break;
+ default:
+ pattrib->iv_len = 0;
+ pattrib->icv_len = 0;
+ break;
+ }
+
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+ ("update_attrib: encrypt=%d securitypriv.sw_encrypt=%d\n",
+ pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+
+ if (pattrib->encrypt &&
+ (padapter->securitypriv.sw_encrypt || !psecuritypriv->hw_decrypted)) {
+ pattrib->bswenc = true;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+ ("update_attrib: encrypt=%d securitypriv.hw_decrypted=%d bswenc = true\n",
+ pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+ } else {
+ pattrib->bswenc = false;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc = false\n"));
+ }
+
+ rtw_set_tx_chksum_offload(pkt, pattrib);
+
+ update_attrib_phy_info(pattrib, psta);
+
+exit:
+
+ return res;
+}
+
+static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ int curfragnum, length;
+ u8 *pframe, *payload, mic[8];
+ struct mic_data micdata;
+ struct sta_info *stainfo;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ struct security_priv *psecuritypriv = &padapter->securitypriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+ u8 hw_hdr_offset = 0;
+ int bmcst = IS_MCAST(pattrib->ra);
+
+ if (pattrib->psta)
+ stainfo = pattrib->psta;
+ else
+ stainfo = rtw_get_stainfo(&padapter->stapriv , &pattrib->ra[0]);
+
+ hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);;
+
+ if (pattrib->encrypt == _TKIP_) {/* if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_PRIVACY_) */
+ /* encode mic code */
+ if (stainfo != NULL) {
+ u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+ 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+ 0x0, 0x0};
+
+ pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+ if (bmcst) {
+ if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16))
+ return _FAIL;
+ /* start to calculate the mic code */
+ rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
+ } else {
+ if (!memcmp(&stainfo->dot11tkiptxmickey.skey[0], null_key, 16)) {
+ /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */
+ /* rtw_msleep_os(10); */
+ return _FAIL;
+ }
+ /* start to calculate the mic code */
+ rtw_secmicsetkey(&micdata, &stainfo->dot11tkiptxmickey.skey[0]);
+ }
+
+ if (pframe[1]&1) { /* ToDS == 1 */
+ rtw_secmicappend(&micdata, &pframe[16], 6); /* DA */
+ if (pframe[1]&2) /* From Ds == 1 */
+ rtw_secmicappend(&micdata, &pframe[24], 6);
+ else
+ rtw_secmicappend(&micdata, &pframe[10], 6);
+ } else { /* ToDS == 0 */
+ rtw_secmicappend(&micdata, &pframe[4], 6); /* DA */
+ if (pframe[1]&2) /* From Ds == 1 */
+ rtw_secmicappend(&micdata, &pframe[16], 6);
+ else
+ rtw_secmicappend(&micdata, &pframe[10], 6);
+ }
+
+ if (pattrib->qos_en)
+ priority[0] = (u8)pxmitframe->attrib.priority;
+
+ rtw_secmicappend(&micdata, &priority[0], 4);
+
+ payload = pframe;
+
+ for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+ payload = (u8 *)RND4((size_t)(payload));
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+ ("=== curfragnum=%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
+ curfragnum, *payload, *(payload+1),
+ *(payload+2), *(payload+3),
+ *(payload+4), *(payload+5),
+ *(payload+6), *(payload+7)));
+
+ payload = payload+pattrib->hdrlen+pattrib->iv_len;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+ ("curfragnum=%d pattrib->hdrlen=%d pattrib->iv_len=%d",
+ curfragnum, pattrib->hdrlen, pattrib->iv_len));
+ if ((curfragnum+1) == pattrib->nr_frags) {
+ length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+ rtw_secmicappend(&micdata, payload, length);
+ payload = payload+length;
+ } else {
+ length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+ rtw_secmicappend(&micdata, payload, length);
+ payload = payload+length+pattrib->icv_len;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum=%d length=%d pattrib->icv_len=%d", curfragnum, length, pattrib->icv_len));
+ }
+ }
+ rtw_secgetmic(&micdata, &(mic[0]));
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: before add mic code!!!\n"));
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: pattrib->last_txcmdsz=%d!!!\n", pattrib->last_txcmdsz));
+ 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\
+ mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
+ mic[0], mic[1], mic[2], mic[3], mic[4], mic[5], mic[6], mic[7]));
+ /* add mic code and add the mic code length in last_txcmdsz */
+
+ memcpy(payload, &(mic[0]), 8);
+ pattrib->last_txcmdsz += 8;
+
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("\n ======== last pkt ========\n"));
+ payload = payload-pattrib->last_txcmdsz+8;
+ for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz; curfragnum = curfragnum+8)
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+ (" %.2x, %.2x, %.2x, %.2x, %.2x, %.2x, %.2x, %.2x ",
+ *(payload+curfragnum), *(payload+curfragnum+1),
+ *(payload+curfragnum+2), *(payload+curfragnum+3),
+ *(payload+curfragnum+4), *(payload+curfragnum+5),
+ *(payload+curfragnum+6), *(payload+curfragnum+7)));
+ } else {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: rtw_get_stainfo==NULL!!!\n"));
+ }
+ }
+
+ return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+ if (pattrib->bswenc) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("### xmitframe_swencrypt\n"));
+ switch (pattrib->encrypt) {
+ case _WEP40_:
+ case _WEP104_:
+ rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
+ break;
+ case _TKIP_:
+ rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
+ break;
+ case _AES_:
+ rtw_aes_encrypt(padapter, (u8 *)pxmitframe);
+ break;
+ default:
+ break;
+ }
+ } else {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, ("### xmitframe_hwencrypt\n"));
+ }
+
+ return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr (struct adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib)
+{
+ u16 *qc;
+
+ struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+ u8 qos_option = false;
+
+ int res = _SUCCESS;
+ __le16 *fctrl = &pwlanhdr->frame_ctl;
+
+ struct sta_info *psta;
+
+ int bmcst = IS_MCAST(pattrib->ra);
+
+ if (pattrib->psta) {
+ psta = pattrib->psta;
+ } else {
+ if (bmcst) {
+ psta = rtw_get_bcmc_stainfo(padapter);
+ } else {
+ psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+ }
+ }
+
+ memset(hdr, 0, WLANHDR_OFFSET);
+
+ SetFrameSubType(fctrl, pattrib->subtype);
+
+ if (pattrib->subtype & WIFI_DATA_TYPE) {
+ if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)) {
+ /* to_ds = 1, fr_ds = 0; */
+ /* Data transfer to AP */
+ SetToDs(fctrl);
+ memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+ memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+
+ if (pqospriv->qos_option)
+ qos_option = true;
+ } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+ /* to_ds = 0, fr_ds = 1; */
+ SetFrDs(fctrl);
+ memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
+ memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+
+ if (psta->qos_option)
+ qos_option = true;
+ } else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+ check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+ memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+ memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+ memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+ if (psta->qos_option)
+ qos_option = true;
+ } else {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
+ res = _FAIL;
+ goto exit;
+ }
+
+ if (pattrib->mdata)
+ SetMData(fctrl);
+
+ if (pattrib->encrypt)
+ SetPrivacy(fctrl);
+
+ if (qos_option) {
+ qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+ if (pattrib->priority)
+ SetPriority(qc, pattrib->priority);
+
+ SetEOSP(qc, pattrib->eosp);
+
+ SetAckpolicy(qc, pattrib->ack_policy);
+ }
+
+ /* TODO: fill HT Control Field */
+
+ /* Update Seq Num will be handled by f/w */
+ if (psta) {
+ psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+ psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+
+ pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+
+ SetSeqNum(hdr, pattrib->seqnum);
+
+ /* check if enable ampdu */
+ if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
+ if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+ pattrib->ampdu_en = true;
+ }
+
+ /* re-check if enable ampdu by BA_starting_seqctrl */
+ if (pattrib->ampdu_en) {
+ u16 tx_seq;
+
+ tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+
+ /* check BA_starting_seqctrl */
+ if (SN_LESS(pattrib->seqnum, tx_seq)) {
+ pattrib->ampdu_en = false;/* AGG BK */
+ } else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
+ psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
+
+ pattrib->ampdu_en = true;/* AGG EN */
+ } else {
+ psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
+ pattrib->ampdu_en = true;/* AGG EN */
+ }
+ }
+ }
+ }
+exit:
+
+ return res;
+}
+
+s32 rtw_txframes_pending(struct adapter *padapter)
+{
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ return (!list_empty(&pxmitpriv->be_pending.queue) ||
+ !list_empty(&pxmitpriv->bk_pending.queue) ||
+ !list_empty(&pxmitpriv->vi_pending.queue) ||
+ !list_empty(&pxmitpriv->vo_pending.queue));
+}
+
+s32 rtw_txframes_sta_ac_pending(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+ struct sta_info *psta;
+ struct tx_servq *ptxservq;
+ int priority = pattrib->priority;
+
+ psta = pattrib->psta;
+
+ switch (priority) {
+ case 1:
+ case 2:
+ ptxservq = &(psta->sta_xmitpriv.bk_q);
+ break;
+ case 4:
+ case 5:
+ ptxservq = &(psta->sta_xmitpriv.vi_q);
+ break;
+ case 6:
+ case 7:
+ ptxservq = &(psta->sta_xmitpriv.vo_q);
+ break;
+ case 0:
+ case 3:
+ default:
+ ptxservq = &(psta->sta_xmitpriv.be_q);
+ break;
+ }
+
+ if (ptxservq)
+ return ptxservq->qcnt;
+ return 0;
+}
+
+/*
+ * Calculate wlan 802.11 packet MAX size from pkt_attrib
+ * This function doesn't consider fragment case
+ */
+u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
+{
+ u32 len = 0;
+
+ len = pattrib->hdrlen + pattrib->iv_len; /* WLAN Header and IV */
+ len += SNAP_SIZE + sizeof(u16); /* LLC */
+ len += pattrib->pktlen;
+ if (pattrib->encrypt == _TKIP_)
+ len += 8; /* MIC */
+ len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /* ICV */
+
+ return len;
+}
+
+/*
+
+This sub-routine will perform all the following:
+
+1. remove 802.3 header.
+2. create wlan_header, based on the info in pxmitframe
+3. append sta's iv/ext-iv
+4. append LLC
+5. move frag chunk from pframe to pxmitframe->mem
+6. apply sw-encrypt, if necessary.
+
+*/
+s32 rtw_xmitframe_coalesce(struct adapter *padapter, struct sk_buff *pkt, struct xmit_frame *pxmitframe)
+{
+ struct pkt_file pktfile;
+ s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+ size_t addr;
+ u8 *pframe, *mem_start;
+ u8 hw_hdr_offset;
+ struct sta_info *psta;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ u8 *pbuf_start;
+ s32 bmcst = IS_MCAST(pattrib->ra);
+ s32 res = _SUCCESS;
+
+ if (!pkt)
+ return _FAIL;
+
+ psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+ if (psta == NULL)
+ return _FAIL;
+
+ if (pxmitframe->buf_addr == NULL) {
+ DBG_88E("==> %s buf_addr == NULL\n", __func__);
+ return _FAIL;
+ }
+
+ pbuf_start = pxmitframe->buf_addr;
+
+ hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+
+ mem_start = pbuf_start + hw_hdr_offset;
+
+ if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
+ DBG_88E("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+ res = _FAIL;
+ goto exit;
+ }
+
+ _rtw_open_pktfile(pkt, &pktfile);
+ _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+ frg_inx = 0;
+ frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
+
+ while (1) {
+ llc_sz = 0;
+
+ mpdu_len = frg_len;
+
+ pframe = mem_start;
+
+ SetMFrag(mem_start);
+
+ pframe += pattrib->hdrlen;
+ mpdu_len -= pattrib->hdrlen;
+
+ /* adding icv, if necessary... */
+ if (pattrib->iv_len) {
+ if (psta != NULL) {
+ switch (pattrib->encrypt) {
+ case _WEP40_:
+ case _WEP104_:
+ WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+ break;
+ case _TKIP_:
+ if (bmcst)
+ TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+ else
+ TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+ break;
+ case _AES_:
+ if (bmcst)
+ AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+ else
+ AES_IV(pattrib->iv, psta->dot11txpn, 0);
+ break;
+ }
+ }
+
+ memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
+ ("rtw_xmitframe_coalesce: keyid=%d pattrib->iv[3]=%.2x pframe=%.2x %.2x %.2x %.2x\n",
+ padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
+
+ pframe += pattrib->iv_len;
+
+ mpdu_len -= pattrib->iv_len;
+ }
+
+ if (frg_inx == 0) {
+ llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+ pframe += llc_sz;
+ mpdu_len -= llc_sz;
+ }
+
+ if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+ mpdu_len -= pattrib->icv_len;
+ }
+
+ if (bmcst) {
+ /* don't do fragment to broadcat/multicast packets */
+ mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+ } else {
+ mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+ }
+
+ pframe += mem_sz;
+
+ if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+ memcpy(pframe, pattrib->icv, pattrib->icv_len);
+ pframe += pattrib->icv_len;
+ }
+
+ frg_inx++;
+
+ if (bmcst || rtw_endofpktfile(&pktfile)) {
+ pattrib->nr_frags = frg_inx;
+
+ pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz : 0) +
+ ((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+
+ ClearMFrag(mem_start);
+
+ break;
+ } else {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
+ }
+
+ addr = (size_t)(pframe);
+
+ mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
+ memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
+ }
+
+ if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+ DBG_88E("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
+ res = _FAIL;
+ goto exit;
+ }
+
+ xmitframe_swencrypt(padapter, pxmitframe);
+
+ if (!bmcst)
+ update_attrib_vcs_info(padapter, pxmitframe);
+ else
+ pattrib->vcs_mode = NONE_VCS;
+
+exit:
+
+ return res;
+}
+
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ * Organizationally Unique Identifier(OUI), 3 octets,
+ * type, defined by that organization, 2 octets.
+ */
+s32 rtw_put_snap(u8 *data, u16 h_proto)
+{
+ struct ieee80211_snap_hdr *snap;
+ u8 *oui;
+
+ snap = (struct ieee80211_snap_hdr *)data;
+ snap->dsap = 0xaa;
+ snap->ssap = 0xaa;
+ snap->ctrl = 0x03;
+
+ if (h_proto == 0x8137 || h_proto == 0x80f3)
+ oui = P802_1H_OUI;
+ else
+ oui = RFC1042_OUI;
+
+ snap->oui[0] = oui[0];
+ snap->oui[1] = oui[1];
+ snap->oui[2] = oui[2];
+
+ *(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+ return SNAP_SIZE + sizeof(u16);
+}
+
+void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len)
+{
+ uint protection;
+ u8 *perp;
+ int erp_len;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct registry_priv *pregistrypriv = &padapter->registrypriv;
+
+ switch (pxmitpriv->vcs_setting) {
+ case DISABLE_VCS:
+ pxmitpriv->vcs = NONE_VCS;
+ break;
+ case ENABLE_VCS:
+ break;
+ case AUTO_VCS:
+ default:
+ perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
+ if (perp == NULL) {
+ pxmitpriv->vcs = NONE_VCS;
+ } else {
+ protection = (*(perp + 2)) & BIT(1);
+ if (protection) {
+ if (pregistrypriv->vcs_type == RTS_CTS)
+ pxmitpriv->vcs = RTS_CTS;
+ else
+ pxmitpriv->vcs = CTS_TO_SELF;
+ } else {
+ pxmitpriv->vcs = NONE_VCS;
+ }
+ }
+ break;
+ }
+
+}
+
+void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz)
+{
+ struct sta_info *psta = NULL;
+ struct stainfo_stats *pstats = NULL;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+ if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
+ pxmitpriv->tx_bytes += sz;
+ pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pxmitframe->agg_num;
+
+ psta = pxmitframe->attrib.psta;
+ if (psta) {
+ pstats = &psta->sta_stats;
+ pstats->tx_pkts += pxmitframe->agg_num;
+ pstats->tx_bytes += sz;
+ }
+ }
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+ struct xmit_buf *pxmitbuf = NULL;
+ struct list_head *plist, *phead;
+ struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+ unsigned long flags;
+
+ spin_lock_irqsave(&pfree_queue->lock, flags);
+
+ if (list_empty(&pfree_queue->queue)) {
+ pxmitbuf = NULL;
+ } else {
+ phead = get_list_head(pfree_queue);
+
+ plist = phead->next;
+
+ pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+ list_del_init(&(pxmitbuf->list));
+ }
+
+ if (pxmitbuf != NULL) {
+ pxmitpriv->free_xmit_extbuf_cnt--;
+
+ pxmitbuf->priv_data = NULL;
+ /* pxmitbuf->ext_tag = true; */
+
+ if (pxmitbuf->sctx) {
+ DBG_88E("%s pxmitbuf->sctx is not NULL\n", __func__);
+ rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+ }
+ }
+
+ spin_unlock_irqrestore(&pfree_queue->lock, flags);
+
+ return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+ struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+ unsigned long flags;
+
+ if (pxmitbuf == NULL)
+ return _FAIL;
+
+ spin_lock_irqsave(&pfree_queue->lock, flags);
+
+ list_del_init(&pxmitbuf->list);
+
+ list_add_tail(&(pxmitbuf->list), get_list_head(pfree_queue));
+ pxmitpriv->free_xmit_extbuf_cnt++;
+
+ spin_unlock_irqrestore(&pfree_queue->lock, flags);
+
+ return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+ struct xmit_buf *pxmitbuf = NULL;
+ struct list_head *plist, *phead;
+ struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+ unsigned long flags;
+
+ /* DBG_88E("+rtw_alloc_xmitbuf\n"); */
+
+ spin_lock_irqsave(&pfree_xmitbuf_queue->lock, flags);
+
+ if (list_empty(&pfree_xmitbuf_queue->queue)) {
+ pxmitbuf = NULL;
+ } else {
+ phead = get_list_head(pfree_xmitbuf_queue);
+
+ plist = phead->next;
+
+ pxmitbuf = container_of(plist, struct xmit_buf, list);
+
+ list_del_init(&(pxmitbuf->list));
+ }
+
+ if (pxmitbuf != NULL) {
+ pxmitpriv->free_xmitbuf_cnt--;
+ pxmitbuf->priv_data = NULL;
+ if (pxmitbuf->sctx) {
+ DBG_88E("%s pxmitbuf->sctx is not NULL\n", __func__);
+ rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+ }
+ }
+ spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, flags);
+
+ return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+ struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+ unsigned long flags;
+
+ if (pxmitbuf == NULL)
+ return _FAIL;
+
+ if (pxmitbuf->sctx) {
+ DBG_88E("%s pxmitbuf->sctx is not NULL\n", __func__);
+ rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+ }
+
+ if (pxmitbuf->ext_tag) {
+ rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+ } else {
+ spin_lock_irqsave(&pfree_xmitbuf_queue->lock, flags);
+
+ list_del_init(&pxmitbuf->list);
+
+ list_add_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue));
+
+ pxmitpriv->free_xmitbuf_cnt++;
+ spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, flags);
+ }
+
+ return _SUCCESS;
+}
+
+/*
+Calling context:
+1. OS_TXENTRY
+2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+
+If we turn on USE_RXTHREAD, then, no need for critical section.
+Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+
+Must be very very cautious...
+
+*/
+
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
+{
+ /*
+ Please remember to use all the osdep_service api,
+ and lock/unlock or _enter/_exit critical to protect
+ pfree_xmit_queue
+ */
+
+ struct xmit_frame *pxframe = NULL;
+ struct list_head *plist, *phead;
+ struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+ spin_lock_bh(&pfree_xmit_queue->lock);
+
+ if (list_empty(&pfree_xmit_queue->queue)) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
+ pxframe = NULL;
+ } else {
+ phead = get_list_head(pfree_xmit_queue);
+
+ plist = phead->next;
+
+ pxframe = container_of(plist, struct xmit_frame, list);
+
+ list_del_init(&(pxframe->list));
+ }
+
+ if (pxframe != NULL) { /* default value setting */
+ pxmitpriv->free_xmitframe_cnt--;
+
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
+
+ pxframe->buf_addr = NULL;
+ pxframe->pxmitbuf = NULL;
+
+ memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
+ /* pxframe->attrib.psta = NULL; */
+
+ pxframe->frame_tag = DATA_FRAMETAG;
+
+ pxframe->pkt = NULL;
+ pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */
+
+ pxframe->agg_num = 1;
+ pxframe->ack_report = 0;
+ }
+
+ spin_unlock_bh(&pfree_xmit_queue->lock);
+
+ return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+ struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+ struct adapter *padapter = pxmitpriv->adapter;
+ struct sk_buff *pndis_pkt = NULL;
+
+ if (pxmitframe == NULL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("====== rtw_free_xmitframe():pxmitframe == NULL!!!!!!!!!!\n"));
+ goto exit;
+ }
+
+ spin_lock_bh(&pfree_xmit_queue->lock);
+
+ list_del_init(&pxmitframe->list);
+
+ if (pxmitframe->pkt) {
+ pndis_pkt = pxmitframe->pkt;
+ pxmitframe->pkt = NULL;
+ }
+
+ list_add_tail(&pxmitframe->list, get_list_head(pfree_xmit_queue));
+
+ pxmitpriv->free_xmitframe_cnt++;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
+
+ spin_unlock_bh(&pfree_xmit_queue->lock);
+
+ if (pndis_pkt)
+ rtw_os_pkt_complete(padapter, pndis_pkt);
+
+exit:
+
+ return _SUCCESS;
+}
+
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue)
+{
+ struct list_head *plist, *phead;
+ struct xmit_frame *pxmitframe;
+
+ spin_lock_bh(&pframequeue->lock);
+
+ phead = get_list_head(pframequeue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ pxmitframe = container_of(plist, struct xmit_frame, list);
+
+ plist = plist->next;
+
+ rtw_free_xmitframe(pxmitpriv, pxmitframe);
+ }
+ spin_unlock_bh(&pframequeue->lock);
+
+}
+
+s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+ ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
+/* pxmitframe->pkt = NULL; */
+ return _FAIL;
+ }
+
+ return _SUCCESS;
+}
+
+static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, struct __queue *pframe_queue)
+{
+ struct list_head *xmitframe_plist, *xmitframe_phead;
+ struct xmit_frame *pxmitframe = NULL;
+
+ xmitframe_phead = get_list_head(pframe_queue);
+ xmitframe_plist = xmitframe_phead->next;
+
+ if (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ list_del_init(&pxmitframe->list);
+
+ ptxservq->qcnt--;
+ }
+ return pxmitframe;
+}
+
+struct xmit_frame *rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, int entry)
+{
+ struct list_head *sta_plist, *sta_phead;
+ struct hw_xmit *phwxmit;
+ struct tx_servq *ptxservq = NULL;
+ struct __queue *pframe_queue = NULL;
+ struct xmit_frame *pxmitframe = NULL;
+ struct adapter *padapter = pxmitpriv->adapter;
+ struct registry_priv *pregpriv = &padapter->registrypriv;
+ int i, inx[4];
+
+ inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+ if (pregpriv->wifi_spec == 1) {
+ int j;
+
+ for (j = 0; j < 4; j++)
+ inx[j] = pxmitpriv->wmm_para_seq[j];
+ }
+
+ spin_lock_bh(&pxmitpriv->lock);
+
+ for (i = 0; i < entry; i++) {
+ phwxmit = phwxmit_i + inx[i];
+
+ sta_phead = get_list_head(phwxmit->sta_queue);
+ sta_plist = sta_phead->next;
+
+ while (sta_phead != sta_plist) {
+ ptxservq = container_of(sta_plist, struct tx_servq, tx_pending);
+
+ pframe_queue = &ptxservq->sta_pending;
+
+ pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
+
+ if (pxmitframe) {
+ phwxmit->accnt--;
+
+ /* Remove sta node when there are no pending packets. */
+ if (list_empty(&pframe_queue->queue)) /* must be done after get_next and before break */
+ list_del_init(&ptxservq->tx_pending);
+ goto exit;
+ }
+
+ sta_plist = sta_plist->next;
+ }
+ }
+exit:
+ spin_unlock_bh(&pxmitpriv->lock);
+
+ return pxmitframe;
+}
+
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, int up, u8 *ac)
+{
+ struct tx_servq *ptxservq;
+
+ switch (up) {
+ case 1:
+ case 2:
+ ptxservq = &(psta->sta_xmitpriv.bk_q);
+ *(ac) = 3;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BK\n"));
+ break;
+ case 4:
+ case 5:
+ ptxservq = &(psta->sta_xmitpriv.vi_q);
+ *(ac) = 1;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VI\n"));
+ break;
+ case 6:
+ case 7:
+ ptxservq = &(psta->sta_xmitpriv.vo_q);
+ *(ac) = 0;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VO\n"));
+ break;
+ case 0:
+ case 3:
+ default:
+ ptxservq = &(psta->sta_xmitpriv.be_q);
+ *(ac) = 2;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BE\n"));
+ break;
+ }
+
+ return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ u8 ac_index;
+ struct sta_info *psta;
+ struct tx_servq *ptxservq;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits;
+ int res = _SUCCESS;
+
+ if (pattrib->psta) {
+ psta = pattrib->psta;
+ } else {
+ psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+ }
+
+ if (psta == NULL) {
+ res = _FAIL;
+ DBG_88E("rtw_xmit_classifier: psta == NULL\n");
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmit_classifier: psta == NULL\n"));
+ goto exit;
+ }
+
+ ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+ if (list_empty(&ptxservq->tx_pending))
+ list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+
+ list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+ ptxservq->qcnt++;
+ phwxmits[ac_index].accnt++;
+exit:
+
+ return res;
+}
+
+void rtw_alloc_hwxmits(struct adapter *padapter)
+{
+ struct hw_xmit *hwxmits;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+
+ pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry);
+
+ hwxmits = pxmitpriv->hwxmits;
+
+ if (pxmitpriv->hwxmit_entry == 5) {
+ hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
+ hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
+ hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
+ hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+ hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
+ } else if (pxmitpriv->hwxmit_entry == 4) {
+ hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+ hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+ hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+ hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+ } else {
+ }
+}
+
+void rtw_free_hwxmits(struct adapter *padapter)
+{
+ struct hw_xmit *hwxmits;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ hwxmits = pxmitpriv->hwxmits;
+ kfree(hwxmits);
+}
+
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, int entry)
+{
+ int i;
+
+ for (i = 0; i < entry; i++, phwxmit++)
+ phwxmit->accnt = 0;
+
+}
+
+static int rtw_br_client_tx(struct adapter *padapter, struct sk_buff **pskb)
+{
+ struct sk_buff *skb = *pskb;
+ int res, is_vlan_tag = 0, i, do_nat25 = 1;
+ unsigned short vlan_hdr = 0;
+ void *br_port = NULL;
+
+ rcu_read_lock();
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+ br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+#else
+ br_port = rcu_dereference(padapter->pnetdev->br_port);
+#endif
+ rcu_read_unlock();
+ spin_lock_bh(&padapter->br_ext_lock);
+ if (!(skb->data[0] & 1) && br_port &&
+ memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+ *((__be16 *)(skb->data+MACADDRLEN*2)) != __constant_htons(ETH_P_8021Q) &&
+ *((__be16 *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP) &&
+ !memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN) && padapter->scdb_entry) {
+ memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+ padapter->scdb_entry->ageing_timer = jiffies;
+ spin_unlock_bh(&padapter->br_ext_lock);
+ } else {
+ if (*((__be16 *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_8021Q)) {
+ is_vlan_tag = 1;
+ vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
+ skb_pull(skb, 4);
+ }
+ if (!memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+ (*((__be16 *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)))
+ memcpy(padapter->br_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
+
+ if (*((__be16 *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)) {
+ if (memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN)) {
+ padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter,
+ skb->data+MACADDRLEN, skb->data+WLAN_ETHHDR_LEN+12);
+ if (padapter->scdb_entry) {
+ memcpy(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN);
+ memcpy(padapter->scdb_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
+ padapter->scdb_entry->ageing_timer = jiffies;
+ do_nat25 = 0;
+ }
+ } else {
+ if (padapter->scdb_entry) {
+ padapter->scdb_entry->ageing_timer = jiffies;
+ do_nat25 = 0;
+ } else {
+ memset(padapter->scdb_mac, 0, MACADDRLEN);
+ memset(padapter->scdb_ip, 0, 4);
+ }
+ }
+ }
+ spin_unlock_bh(&padapter->br_ext_lock);
+ if (do_nat25) {
+ if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) {
+ struct sk_buff *newskb;
+
+ if (is_vlan_tag) {
+ skb_push(skb, 4);
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+ *((__be16 *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
+ *((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
+ }
+
+ newskb = skb_copy(skb, GFP_ATOMIC);
+ if (newskb == NULL) {
+ DEBUG_ERR("TX DROP: skb_copy fail!\n");
+ return -1;
+ }
+ dev_kfree_skb_any(skb);
+
+ *pskb = skb = newskb;
+ if (is_vlan_tag) {
+ vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
+ skb_pull(skb, 4);
+ }
+ }
+
+ if (skb_is_nonlinear(skb))
+ DEBUG_ERR("%s(): skb_is_nonlinear!!\n", __func__);
+
+ res = skb_linearize(skb);
+ if (res < 0) {
+ DEBUG_ERR("TX DROP: skb_linearize fail!\n");
+ return -1;
+ }
+
+ res = nat25_db_handle(padapter, skb, NAT25_INSERT);
+ if (res < 0) {
+ if (res == -2) {
+ DEBUG_ERR("TX DROP: nat25_db_handle fail!\n");
+ return -1;
+ }
+ return 0;
+ }
+ }
+
+ memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+
+ dhcp_flag_bcast(padapter, skb);
+
+ if (is_vlan_tag) {
+ skb_push(skb, 4);
+ for (i = 0; i < 6; i++)
+ *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+ *((__be16 *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
+ *((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
+ }
+ }
+
+ /* check if SA is equal to our MAC */
+ if (memcmp(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN)) {
+ DEBUG_ERR("TX DROP: untransformed frame SA:%02X%02X%02X%02X%02X%02X!\n",
+ skb->data[6], skb->data[7], skb->data[8], skb->data[9], skb->data[10], skb->data[11]);
+ return -1;
+ }
+ return 0;
+}
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+ u32 addr;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+ switch (pattrib->qsel) {
+ case 0:
+ case 3:
+ addr = BE_QUEUE_INX;
+ break;
+ case 1:
+ case 2:
+ addr = BK_QUEUE_INX;
+ break;
+ case 4:
+ case 5:
+ addr = VI_QUEUE_INX;
+ break;
+ case 6:
+ case 7:
+ addr = VO_QUEUE_INX;
+ break;
+ case 0x10:
+ addr = BCN_QUEUE_INX;
+ break;
+ case 0x11:/* BC/MC in PS (HIQ) */
+ addr = HIGH_QUEUE_INX;
+ break;
+ case 0x12:
+ default:
+ addr = MGT_QUEUE_INX;
+ break;
+ }
+
+ return addr;
+}
+
+static void do_queue_select(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+ u8 qsel;
+
+ qsel = pattrib->priority;
+ RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority=%d , qsel = %d\n", pattrib->priority , qsel));
+
+ pattrib->qsel = qsel;
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ * 1 enqueue
+ * 0 success, hardware will handle this xmit frame(packet)
+ * <0 fail
+ */
+s32 rtw_xmit(struct adapter *padapter, struct sk_buff **ppkt)
+{
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+ struct xmit_frame *pxmitframe = NULL;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ void *br_port = NULL;
+ s32 res;
+
+ pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+ if (pxmitframe == NULL) {
+ RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
+ DBG_88E("DBG_TX_DROP_FRAME %s no more pxmitframe\n", __func__);
+ return -1;
+ }
+
+ rcu_read_lock();
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,36)
+ br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+#else
+ br_port = rcu_dereference(padapter->pnetdev->br_port);
+#endif
+ rcu_read_unlock();
+
+ if (br_port && check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
+ res = rtw_br_client_tx(padapter, ppkt);
+ if (res == -1) {
+ rtw_free_xmitframe(pxmitpriv, pxmitframe);
+ return -1;
+ }
+ }
+
+ res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+ if (res == _FAIL) {
+ RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
+ rtw_free_xmitframe(pxmitpriv, pxmitframe);
+ return -1;
+ }
+ pxmitframe->pkt = *ppkt;
+
+ rtw_led_control(padapter, LED_CTL_TX);
+
+ do_queue_select(padapter, &pxmitframe->attrib);
+
+#ifdef CONFIG_88EU_AP_MODE
+ spin_lock_bh(&pxmitpriv->lock);
+ if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe)) {
+ spin_unlock_bh(&pxmitpriv->lock);
+ return 1;
+ }
+ spin_unlock_bh(&pxmitpriv->lock);
+#endif
+
+ if (rtw_hal_xmit(padapter, pxmitframe) == false)
+ return 1;
+
+ return 0;
+}
+
+#if defined(CONFIG_88EU_AP_MODE)
+
+int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+ int ret = false;
+ struct sta_info *psta = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct pkt_attrib *pattrib = &pxmitframe->attrib;
+ struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+ int bmcst = IS_MCAST(pattrib->ra);
+
+ if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false)
+ return ret;
+
+ if (pattrib->psta)
+ psta = pattrib->psta;
+ else
+ psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+
+ if (psta == NULL)
+ return ret;
+
+ if (pattrib->triggered == 1) {
+ if (bmcst)
+ pattrib->qsel = 0x11;/* HIQ */
+ return ret;
+ }
+
+ if (bmcst) {
+ spin_lock_bh(&psta->sleep_q.lock);
+
+ if (pstapriv->sta_dz_bitmap) {/* if any one sta is in ps mode */
+ list_del_init(&pxmitframe->list);
+
+ list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+ psta->sleepq_len++;
+
+ pstapriv->tim_bitmap |= BIT(0);/* */
+ pstapriv->sta_dz_bitmap |= BIT(0);
+
+ update_beacon(padapter, _TIM_IE_, NULL, false);/* tx bc/mc packets after upate bcn */
+
+ ret = true;
+ }
+
+ spin_unlock_bh(&psta->sleep_q.lock);
+
+ return ret;
+ }
+
+ spin_lock_bh(&psta->sleep_q.lock);
+
+ if (psta->state&WIFI_SLEEP_STATE) {
+ u8 wmmps_ac = 0;
+
+ if (pstapriv->sta_dz_bitmap&BIT(psta->aid)) {
+ list_del_init(&pxmitframe->list);
+
+ list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+ psta->sleepq_len++;
+
+ switch (pattrib->priority) {
+ case 1:
+ case 2:
+ wmmps_ac = psta->uapsd_bk&BIT(0);
+ break;
+ case 4:
+ case 5:
+ wmmps_ac = psta->uapsd_vi&BIT(0);
+ break;
+ case 6:
+ case 7:
+ wmmps_ac = psta->uapsd_vo&BIT(0);
+ break;
+ case 0:
+ case 3:
+ default:
+ wmmps_ac = psta->uapsd_be&BIT(0);
+ break;
+ }
+
+ if (wmmps_ac)
+ psta->sleepq_ac_len++;
+
+ if (((psta->has_legacy_ac) && (!wmmps_ac)) ||
+ ((!psta->has_legacy_ac) && (wmmps_ac))) {
+ pstapriv->tim_bitmap |= BIT(psta->aid);
+
+ if (psta->sleepq_len == 1) {
+ /* upate BCN for TIM IE */
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+ }
+ }
+ ret = true;
+ }
+ }
+
+ spin_unlock_bh(&psta->sleep_q.lock);
+
+ return ret;
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue)
+{
+ struct list_head *plist, *phead;
+ u8 ac_index;
+ struct tx_servq *ptxservq;
+ struct pkt_attrib *pattrib;
+ struct xmit_frame *pxmitframe;
+ struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits;
+
+ phead = get_list_head(pframequeue);
+ plist = phead->next;
+
+ while (phead != plist) {
+ pxmitframe = container_of(plist, struct xmit_frame, list);
+
+ plist = plist->next;
+
+ xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
+
+ pattrib = &pxmitframe->attrib;
+
+ ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+ ptxservq->qcnt--;
+ phwxmits[ac_index].accnt--;
+ }
+}
+
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+ struct sta_info *psta_bmc;
+ struct sta_xmit_priv *pstaxmitpriv;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+ struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+ pstaxmitpriv = &psta->sta_xmitpriv;
+
+ /* for BC/MC Frames */
+ psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+ spin_lock_bh(&pxmitpriv->lock);
+
+ psta->state |= WIFI_SLEEP_STATE;
+
+ pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+
+ dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
+ list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+
+ dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
+ list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+
+ dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
+ list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+
+ dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
+ list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+
+ /* for BC/MC Frames */
+ pstaxmitpriv = &psta_bmc->sta_xmitpriv;
+ dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
+ list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+
+ spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+ u8 update_mask = 0, wmmps_ac = 0;
+ struct sta_info *psta_bmc;
+ struct list_head *xmitframe_plist, *xmitframe_phead;
+ struct xmit_frame *pxmitframe = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ spin_lock_bh(&psta->sleep_q.lock);
+
+ xmitframe_phead = get_list_head(&psta->sleep_q);
+ xmitframe_plist = xmitframe_phead->next;
+
+ while (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ list_del_init(&pxmitframe->list);
+
+ switch (pxmitframe->attrib.priority) {
+ case 1:
+ case 2:
+ wmmps_ac = psta->uapsd_bk&BIT(1);
+ break;
+ case 4:
+ case 5:
+ wmmps_ac = psta->uapsd_vi&BIT(1);
+ break;
+ case 6:
+ case 7:
+ wmmps_ac = psta->uapsd_vo&BIT(1);
+ break;
+ case 0:
+ case 3:
+ default:
+ wmmps_ac = psta->uapsd_be&BIT(1);
+ break;
+ }
+
+ psta->sleepq_len--;
+ if (psta->sleepq_len > 0)
+ pxmitframe->attrib.mdata = 1;
+ else
+ pxmitframe->attrib.mdata = 0;
+
+ if (wmmps_ac) {
+ psta->sleepq_ac_len--;
+ if (psta->sleepq_ac_len > 0) {
+ pxmitframe->attrib.mdata = 1;
+ pxmitframe->attrib.eosp = 0;
+ } else {
+ pxmitframe->attrib.mdata = 0;
+ pxmitframe->attrib.eosp = 1;
+ }
+ }
+
+ pxmitframe->attrib.triggered = 1;
+
+ spin_unlock_bh(&psta->sleep_q.lock);
+ if (rtw_hal_xmit(padapter, pxmitframe))
+ rtw_os_xmit_complete(padapter, pxmitframe);
+ spin_lock_bh(&psta->sleep_q.lock);
+ }
+
+ if (psta->sleepq_len == 0) {
+ pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+ update_mask = BIT(0);
+
+ if (psta->state&WIFI_SLEEP_STATE)
+ psta->state ^= WIFI_SLEEP_STATE;
+
+ if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+ psta->expire_to = pstapriv->expire_to;
+ psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+ }
+
+ pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+ }
+
+ spin_unlock_bh(&psta->sleep_q.lock);
+
+ /* for BC/MC Frames */
+ psta_bmc = rtw_get_bcmc_stainfo(padapter);
+ if (!psta_bmc)
+ return;
+
+ if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) { /* no any sta in ps mode */
+ spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+ xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+ xmitframe_plist = xmitframe_phead->next;
+
+ while (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ list_del_init(&pxmitframe->list);
+
+ psta_bmc->sleepq_len--;
+ if (psta_bmc->sleepq_len > 0)
+ pxmitframe->attrib.mdata = 1;
+ else
+ pxmitframe->attrib.mdata = 0;
+
+ pxmitframe->attrib.triggered = 1;
+
+ spin_unlock_bh(&psta_bmc->sleep_q.lock);
+ if (rtw_hal_xmit(padapter, pxmitframe))
+ rtw_os_xmit_complete(padapter, pxmitframe);
+ spin_lock_bh(&psta_bmc->sleep_q.lock);
+ }
+
+ if (psta_bmc->sleepq_len == 0) {
+ pstapriv->tim_bitmap &= ~BIT(0);
+ pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+ update_mask |= BIT(1);
+ }
+
+ spin_unlock_bh(&psta_bmc->sleep_q.lock);
+ }
+
+ if (update_mask)
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+}
+
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta)
+{
+ u8 wmmps_ac = 0;
+ struct list_head *xmitframe_plist, *xmitframe_phead;
+ struct xmit_frame *pxmitframe = NULL;
+ struct sta_priv *pstapriv = &padapter->stapriv;
+
+ spin_lock_bh(&psta->sleep_q.lock);
+
+ xmitframe_phead = get_list_head(&psta->sleep_q);
+ xmitframe_plist = xmitframe_phead->next;
+
+ while (xmitframe_phead != xmitframe_plist) {
+ pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
+
+ xmitframe_plist = xmitframe_plist->next;
+
+ switch (pxmitframe->attrib.priority) {
+ case 1:
+ case 2:
+ wmmps_ac = psta->uapsd_bk&BIT(1);
+ break;
+ case 4:
+ case 5:
+ wmmps_ac = psta->uapsd_vi&BIT(1);
+ break;
+ case 6:
+ case 7:
+ wmmps_ac = psta->uapsd_vo&BIT(1);
+ break;
+ case 0:
+ case 3:
+ default:
+ wmmps_ac = psta->uapsd_be&BIT(1);
+ break;
+ }
+
+ if (!wmmps_ac)
+ continue;
+
+ list_del_init(&pxmitframe->list);
+
+ psta->sleepq_len--;
+ psta->sleepq_ac_len--;
+
+ if (psta->sleepq_ac_len > 0) {
+ pxmitframe->attrib.mdata = 1;
+ pxmitframe->attrib.eosp = 0;
+ } else {
+ pxmitframe->attrib.mdata = 0;
+ pxmitframe->attrib.eosp = 1;
+ }
+
+ pxmitframe->attrib.triggered = 1;
+
+ if (rtw_hal_xmit(padapter, pxmitframe) == true)
+ rtw_os_xmit_complete(padapter, pxmitframe);
+
+ if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) {
+ pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+ /* upate BCN for TIM IE */
+ update_beacon(padapter, _TIM_IE_, NULL, false);
+ }
+ }
+
+ spin_unlock_bh(&psta->sleep_q.lock);
+}
+
+#endif
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
+{
+ sctx->timeout_ms = timeout_ms;
+ sctx->submit_time = jiffies;
+ init_completion(&sctx->done);
+ sctx->status = RTW_SCTX_SUBMITTED;
+}
+
+int rtw_sctx_wait(struct submit_ctx *sctx)
+{
+ int ret = _FAIL;
+ unsigned long expire;
+ int status = 0;
+
+ expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
+ if (!wait_for_completion_timeout(&sctx->done, expire)) {
+ /* timeout, do something?? */
+ status = RTW_SCTX_DONE_TIMEOUT;
+ DBG_88E("%s timeout\n", __func__);
+ } else {
+ status = sctx->status;
+ }
+
+ if (status == RTW_SCTX_DONE_SUCCESS)
+ ret = _SUCCESS;
+
+ return ret;
+}
+
+static bool rtw_sctx_chk_waring_status(int status)
+{
+ switch (status) {
+ case RTW_SCTX_DONE_UNKNOWN:
+ case RTW_SCTX_DONE_BUF_ALLOC:
+ case RTW_SCTX_DONE_BUF_FREE:
+
+ case RTW_SCTX_DONE_DRV_STOP:
+ case RTW_SCTX_DONE_DEV_REMOVE:
+ return true;
+ default:
+ return false;
+ }
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+ if (*sctx) {
+ if (rtw_sctx_chk_waring_status(status))
+ DBG_88E("%s status:%d\n", __func__, status);
+ (*sctx)->status = status;
+ complete(&((*sctx)->done));
+ *sctx = NULL;
+ }
+}
+
+void rtw_sctx_done(struct submit_ctx **sctx)
+{
+ rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
+}
+
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+ struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+ pack_tx_ops->submit_time = jiffies;
+ pack_tx_ops->timeout_ms = timeout_ms;
+ pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+ return rtw_sctx_wait(pack_tx_ops);
+}
+
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
+{
+ struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+ if (pxmitpriv->ack_tx)
+ rtw_sctx_done_err(&pack_tx_ops, status);
+ else
+ DBG_88E("%s ack_tx not set\n", __func__);
+}
--
2.31.1
next prev parent reply other threads:[~2021-07-27 23:22 UTC|newest]
Thread overview: 19+ messages / expand[flat|nested] mbox.gz Atom feed top
2021-07-27 23:22 [PATCH v4 0/6] staging: r8188eu: add newer/better RTL8188eu driver Phillip Potter
2021-07-27 23:22 ` Phillip Potter [this message]
2021-07-27 23:22 ` [PATCH v4 2/6] staging: r8188eu: introduce new hal dir for " Phillip Potter
2021-07-27 23:22 ` [PATCH v4 3/6] staging: r8188eu: introduce new os_dep " Phillip Potter
2021-07-27 23:22 ` [PATCH v4 4/6] staging: r8188eu: introduce new include " Phillip Potter
2021-07-27 23:22 ` [PATCH v4 5/6] staging: r8188eu: introduce new supporting files " Phillip Potter
2021-07-27 23:22 ` [PATCH v4 6/6] staging: r8188eu: attach newly imported driver to build system Phillip Potter
2021-07-28 7:46 ` [PATCH v4 0/6] staging: r8188eu: add newer/better RTL8188eu driver Martin Kaiser
2021-07-28 16:00 ` Phillip Potter
2021-07-28 16:00 ` Phillip Potter
2021-07-29 13:37 ` Martin Kaiser
2021-07-29 14:00 ` Greg KH
2021-07-29 16:47 ` Larry Finger
2021-08-01 17:50 ` Martin Kaiser
2021-07-28 18:14 ` Greg KH
2021-07-28 18:41 ` Larry Finger
2021-07-28 18:55 ` Greg KH
2021-07-28 22:17 ` Phillip Potter
2021-07-28 22:17 ` Phillip Potter
Reply instructions:
You may reply publicly to this message via plain-text email
using any one of the following methods:
* Save the following mbox file, import it into your mail client,
and reply-to-all from there: mbox
Avoid top-posting and favor interleaved quoting:
https://en.wikipedia.org/wiki/Posting_style#Interleaved_style
* Reply using the --to, --cc, and --in-reply-to
switches of git-send-email(1):
git send-email \
--in-reply-to=20210727232219.2948-2-phil@philpotter.co.uk \
--to=phil@philpotter.co.uk \
--cc=Larry.Finger@lwfinger.net \
--cc=dan.carpenter@oracle.com \
--cc=fabioaiuto83@gmail.com \
--cc=gregkh@linuxfoundation.org \
--cc=linux-kernel@vger.kernel.org \
--cc=linux-staging@lists.linux.dev \
--cc=martin@kaiser.cx \
/path/to/YOUR_REPLY
https://kernel.org/pub/software/scm/git/docs/git-send-email.html
* If your mail client supports setting the In-Reply-To header
via mailto: links, try the mailto: link
Be sure your reply has a Subject: header at the top and a blank line
before the message body.
This is an external index of several public inboxes,
see mirroring instructions on how to clone and mirror
all data and code used by this external index.