From: Larry Finger Date: Sat, 29 Mar 2014 02:37:38 +0000 (-0500) Subject: staging: r8723au: Add source files for new driver - part 1 X-Git-Url: http://git.lede-project.org./?a=commitdiff_plain;h=5e93f3520;p=openwrt%2Fstaging%2Fblogic.git staging: r8723au: Add source files for new driver - part 1 The Realtek USB device RTL8723AU is found in Lenovo Yoga 13 tablets. A driver for it has been available in a GitHub repo for several months. This commit contains the first part of the source files. The source is arbitrarily split to avoid E-mail files that are too large. Jes Sorensen at RedHat has made many improvements to the vendor code, and he has been doing the testing. I do not have access to this device. Signed-off-by: Larry Finger Cc: Jes Sorensen Signed-off-by: Greg Kroah-Hartman --- diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c new file mode 100644 index 000000000000..3d936eb937ef --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_ap.c @@ -0,0 +1,2087 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_AP_C_ + +#include +#include +#include +#include + +#ifdef CONFIG_8723AU_AP_MODE + +extern unsigned char RTW_WPA_OUI23A[]; +extern unsigned char WMM_OUI23A[]; +extern unsigned char WPS_OUI23A[]; +extern unsigned char P2P_OUI23A[]; +extern unsigned char WFD_OUI23A[]; + +void init_mlme_ap_info23a(struct rtw_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_queue23a(&pacl_list->acl_node_q); + + start_ap_mode23a(padapter); +} + +void free_mlme_ap_info23a(struct rtw_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_flush23a(padapter); + + pmlmeinfo->state = _HW_STATE_NOLINK_; + + /* free_assoc_sta_resources */ + rtw_free_all_stainfo23a(padapter); + + /* free bc/mc sta_info */ + psta = rtw_get_bcmc_stainfo23a(padapter); + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(padapter, psta); + spin_unlock_bh(&pstapriv->sta_hash_lock); +} + +static void update_BCNTIM(struct rtw_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; + u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL; + u16 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_ie23a(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; + + /* calulate head_len */ + offset = _FIXED_IE_LENGTH_; + + /* get ssid_ie len */ + p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)); + if (p != NULL) + offset += tmp_len+2; + + /* get supported rates len */ + p = rtw_get_ie23a(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 = kmalloc(remainder_ielen, GFP_ATOMIC); + 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 peroid */ + + 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_cmd23a(padapter); +} + +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)) + ret = true; + + sta_update_last_rx_pkts(psta); + + return ret; +} + +void expire_timeout_chk23a(struct rtw_adapter *padapter) +{ + struct list_head *phead, *plist, *ptmp; + u8 updated = 0; + struct sta_info *psta; + 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; + + /* check auth_queue */ + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, auth_list); + + if (psta->expire_to>0) { + psta->expire_to--; + if (psta->expire_to == 0) { + list_del_init(&psta->auth_list); + pstapriv->auth_list_cnt--; + + DBG_8723A("auth expire %pM\n", psta->hwaddr); + + spin_unlock_bh(&pstapriv->auth_list_lock); + + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(padapter, psta); + spin_unlock_bh(&pstapriv->sta_hash_lock); + + spin_lock_bh(&pstapriv->auth_list_lock); + } + } + + } + + spin_unlock_bh(&pstapriv->auth_list_lock); + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + + /* check asoc_queue */ + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + if (chk_sta_is_alive(psta) || !psta->expire_to) { + psta->expire_to = pstapriv->expire_to; + psta->keep_alive_trycnt = 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 staion 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 |= CHKBIT(psta->aid); + update_beacon23a(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_offset23a(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_8723A("asoc expire "MAC_FMT", state = 0x%x\n", MAC_ARG(psta->hwaddr), psta->state); + updated = ap_free_sta23a(padapter, psta, false, 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_8723A("%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n", __func__, + MAC_ARG(psta->hwaddr), + psta->sleepq_len, + padapter->xmitpriv.free_xmitframe_cnt, + pstapriv->asoc_list_cnt); + wakeup_sta_to_xmit23a(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_ch23a(padapter) != pmlmeext->cur_channel) { + backup_oper_channel = rtw_get_oper_ch23a(padapter); + SelectChannel23a(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_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]); + if (!(psta->state &_FW_LINKED)) + continue; + + if (psta->state & WIFI_SLEEP_STATE) + ret = issue_nulldata23a(padapter, psta->hwaddr, 0, 1, 50); + else + ret = issue_nulldata23a(padapter, psta->hwaddr, 0, 3, 50); + + psta->keep_alive_trycnt++; + if (ret == _SUCCESS) + { + DBG_8723A("asoc check, sta(" MAC_FMT ") is alive\n", MAC_ARG(psta->hwaddr)); + psta->expire_to = pstapriv->expire_to; + psta->keep_alive_trycnt = 0; + continue; + } + else if (psta->keep_alive_trycnt <= 3) + { + DBG_8723A("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_8723A("asoc expire "MAC_FMT", state = 0x%x\n", MAC_ARG(psta->hwaddr), psta->state); + 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_sta23a(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING); + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + } + + if (backup_oper_channel>0) /* back to the original operation channel */ + SelectChannel23a(padapter, backup_oper_channel); +} + + associated_clients_update23a(padapter, updated); +} + +void add_RATid23a(struct rtw_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; ibssrateset); i++) + { + if (psta->bssrateset[i]) + tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f); + } + /* n mode ra_bitmap */ + if (psta_ht->ht_option) + { + rtw23a_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; iht_cap.mcs.rx_mask[i/8] & BIT(i%8)) + tx_ra_bitmap |= CHKBIT(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_raid23a(sta_band); + init_rate = get_highest_rate_idx23a(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 == true) + arg |= BIT(5); + + tx_ra_bitmap |= ((raid<<28)&0xf0000000); + + DBG_8723A("%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_tid23a(padapter, tx_ra_bitmap, arg, rssi_level); + + if (shortGIrate == true) + init_rate |= BIT(6); + + /* set ra_id, init_rate */ + psta->raid = raid; + psta->init_rate = init_rate; + + } + else + { + DBG_8723A("station aid %d exceed the max number\n", psta->aid); + } +} + +static void update_bmc_sta(struct rtw_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_stainfo23a(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_RATid23a */ + supportRateNum = rtw_get_rateset_len23a((u8*)&pcur_network->SupportedRates); + network_type = rtw_check_network_type23a((u8*)&pcur_network->SupportedRates, supportRateNum, 1); + + memcpy(psta->bssrateset, &pcur_network->SupportedRates, supportRateNum); + psta->bssratelen = supportRateNum; + + /* b/g mode ra_bitmap */ + for (i = 0; ibssrateset[i]) + tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(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_raid23a(network_type); + init_rate = get_highest_rate_idx23a(tx_ra_bitmap&0x0fffffff)&0x3f; + + /* ap mode */ + rtw_hal_set_odm_var23a(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_8723A("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_tid23a(padapter, tx_ra_bitmap, arg, 0); + + } + + /* set ra_id, init_rate */ + psta->raid = raid; + psta->init_rate = init_rate; + + rtw_stassoc_hw_rpt23a(padapter, psta); + + spin_lock_bh(&psta->lock); + psta->state = _FW_LINKED; + spin_unlock_bh(&psta->lock); + + } + else + { + DBG_8723A("add_RATid23a_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_info23a_apmode23a(struct rtw_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; + /* set intf_tag to if1 */ + + psta->mac_id = psta->aid+1; + DBG_8723A("%s\n", __func__); + + /* ap mode */ + rtw_hal_set_odm_var23a(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_update23a(padapter, psta); + /* HT related cap */ + if (phtpriv_sta->ht_option) + { + /* check if sta supports rx ampdu */ + phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable; + + /* check if sta support s Short GI */ + if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20|IEEE80211_HT_CAP_SGI_40)) + phtpriv_sta->sgi = true; + + /* bwmode */ + if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH_20_40)) { + /* phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_40; */ + 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_delba23a(padapter, 0, psta->hwaddr);/* recipient */ + + /* TX AMPDU */ + send_delba23a(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 rtw_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_8723A("%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_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing)); + + rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); + + /* Config SM Power Save setting */ + pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) + DBG_8723A("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__); +} + +static void start_bss_network(struct rtw_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_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_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 (NULL == rtw_get_wps_ie23a(pnetwork->IEs+_FIXED_IE_LENGTH_, pnetwork->IELength-_FIXED_IE_LENGTH_, NULL, NULL)) + pmlmeext->bstart_bss = true; + + /* todo: update wmm, ht cap */ + /* pmlmeinfo->WMM_enable; */ + /* pmlmeinfo->HT_enable; */ + 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_entry23a(padapter); /* clear CAM */ + } + + /* set MSR to AP_Mode */ + Set_MSR23a(padapter, _HW_STATE_AP_); + + /* Set BSSID REG */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pnetwork->MacAddress); + + /* Set EDCA param reg */ + acparm = 0x002F3217; /* VO */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm)); + acparm = 0x005E4317; /* VI */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm)); + acparm = 0x005ea42b; + rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm)); + acparm = 0x0000A444; /* BK */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm)); + + /* Set Security */ + val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf; + rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* Beacon Control related register */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval)); + + UpdateBrateTbl23a(padapter, pnetwork->SupportedRates); + rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates); + + if (!pmlmepriv->cur_network.join_res) { + /* setting only at first time */ + + /* disable dynamic functions, such as high power, DIG */ + + /* turn on all dynamic functions */ + Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); + } + /* set channel, bwmode */ + p = rtw_get_ie23a((pnetwork->IEs + sizeof(struct ndis_802_11_fixed_ies)), + _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - + sizeof(struct ndis_802_11_fixed_ies))); + 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: + /* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; */ + 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_bwmode23a(padapter, cur_channel, cur_ch_offset, cur_bwmode); + + DBG_8723A("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_mode23a(padapter); + + /* udpate capability after cur_wireless_mode updated */ + update_capinfo23a(padapter, rtw_get_capability23a((struct wlan_bssid_ex *)pnetwork)); + + /* let pnetwork_mlmeext == pnetwork_mlme. */ + memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length); + +#ifdef CONFIG_8723AU_P2P + memcpy(pwdinfo->p2p_group_ssid, pnetwork->Ssid.ssid, + pnetwork->Ssid.ssid_len); + pwdinfo->p2p_group_ssid_len = pnetwork->Ssid.ssid_len; +#endif /* CONFIG_8723AU_P2P */ + + if (pmlmeext->bstart_bss) { + update_beacon23a(padapter, _TIM_IE_, NULL, false); + + /* issue beacon frame */ + if (send_beacon23a(padapter) == _FAIL) + DBG_8723A("issue_beacon23a, fail!\n"); + } + + /* update bc/mc sta_info */ + update_bmc_sta(padapter); +} + +int rtw_check_beacon_data23a(struct rtw_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 = &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_8723A("%s, len =%d\n", __func__, len); + + if (!check_fwstate(pmlmepriv, WIFI_AP_STATE)) + 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 */ + /* ie + 8; 8: TimeStamp, 2: Beacon Interval 2:Capability */ + p = rtw_get_beacon_interval23a_from_ie(ie); + pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p); + + /* capability */ + cap = RTW_GET_LE16(ie); + + /* SSID */ + p = rtw_get_ie23a(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 cfg80211_ssid)); + memcpy(pbss_network->Ssid.ssid, (p + 2), ie_len); + pbss_network->Ssid.ssid_len = ie_len; + } + + /* chnnel */ + channel = 0; + pbss_network->Configuration.Length = 0; + p = rtw_get_ie23a(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_ie23a(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p) { + memcpy(supportRate, p+2, ie_len); + supportRateNum = ie_len; + } + + /* get ext_supported rates */ + p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, + &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_); + if (p) { + memcpy(supportRate+supportRateNum, p+2, ie_len); + supportRateNum += ie_len; + } + + network_type = rtw_check_network_type23a(supportRate, + supportRateNum, channel); + + rtw_set_supported_rate23a(pbss_network->SupportedRates, network_type); + + /* parsing ERP_IE */ + p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) + ERP_IE_handler23a(padapter, (struct ndis_802_11_var_ies *)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_ie23a(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + if (rtw_parse_wpa2_ie23a(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_ie23a(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_ie23a(p, ie_len+2, &group_cipher, + &pairwise_cipher, NULL) == _SUCCESS) { + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + + /* psk, todo:802.1x */ + psecuritypriv->dot8021xalg = 1; + + 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_ie23a(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_ie23a(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; + + rtw23a_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_AMPDU_PARM_DENSITY & (0x07<<2)); + else + pht_cap->ampdu_params_info |= (IEEE80211_HT_AMPDU_PARM_DENSITY&0x00); + + /* set Max Rx AMPDU size to 64K */ + pht_cap->ampdu_params_info |= (IEEE80211_HT_AMPDU_PARM_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_ie23a(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; + + /* 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_handler23a(padapter, (struct ndis_802_11_var_ies *)pHT_caps_ie); + + HT_info_handler23a(padapter, (struct ndis_802_11_var_ies *)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_stainfo23a(&padapter->stapriv, pbss_network->MacAddress); + if (!psta) { + psta = rtw_alloc_stainfo23a(&padapter->stapriv, pbss_network->MacAddress); + if (!psta) + return _FAIL; + } + /* fix bug of flush_cam_entry at STOP AP mode */ + psta->state |= WIFI_AP_STATE; + rtw_indicate_connect23a(padapter); + + /* for check if already set beacon */ + pmlmepriv->cur_network.join_res = true; + + return ret; +} + +void rtw_set_macaddr_acl23a(struct rtw_adapter *padapter, int mode) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + + DBG_8723A("%s, mode =%d\n", __func__, mode); + + pacl_list->mode = mode; +} + +int rtw_acl_add_sta23a(struct rtw_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 rtw_queue *pacl_node_q = &pacl_list->acl_node_q; + + DBG_8723A("%s(acl_num =%d) =" MAC_FMT "\n", __func__, pacl_list->num, MAC_ARG(addr)); + + if ((NUM_ACL-1) < pacl_list->num) + return -1; + + spin_lock_bh(&pacl_node_q->lock); + + phead = get_list_head(pacl_node_q); + + list_for_each(plist, phead) { + paclnode = container_of(plist, struct rtw_wlan_acl_node, list); + + if (!memcmp(paclnode->addr, addr, ETH_ALEN)) { + if (paclnode->valid == true) { + added = true; + DBG_8723A("%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_8723A("%s, acl_num =%d\n", __func__, pacl_list->num); + + spin_unlock_bh(&pacl_node_q->lock); + return ret; +} + +int rtw_acl_remove_sta23a(struct rtw_adapter *padapter, u8 *addr) +{ + struct list_head *plist, *phead, *ptmp; + struct rtw_wlan_acl_node *paclnode; + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + struct rtw_queue *pacl_node_q = &pacl_list->acl_node_q; + int ret = 0; + + DBG_8723A("%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); + + list_for_each_safe(plist, ptmp, phead) { + paclnode = container_of(plist, struct rtw_wlan_acl_node, list); + + 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_8723A("%s, acl_num =%d\n", __func__, pacl_list->num); + + return ret; +} + +static void update_bcn_fixed_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_erpinfo_ie(struct rtw_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_8723A("%s, ERP_enable =%d\n", __func__, pmlmeinfo->ERP_enable); + + if (!pmlmeinfo->ERP_enable) + return; + + /* parsing ERP_IE */ + p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (p && len>0) + { + struct ndis_802_11_var_ies * pIE = (struct ndis_802_11_var_ies *)p; + + if (pmlmepriv->num_sta_non_erp == 1) + pIE->data[0] |= WLAN_ERP_NON_ERP_PRESENT | + WLAN_ERP_USE_PROTECTION; + else + pIE->data[0] &= ~(WLAN_ERP_NON_ERP_PRESENT | + WLAN_ERP_USE_PROTECTION); + + if (pmlmepriv->num_sta_no_short_preamble > 0) + pIE->data[0] |= WLAN_ERP_BARKER_PREAMBLE; + else + pIE->data[0] &= ~(WLAN_ERP_BARKER_PREAMBLE); + + ERP_IE_handler23a(padapter, pIE); + } +} + +static void update_bcn_htcap_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_htinfo_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_rsn_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_wpa_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_wmm_ie(struct rtw_adapter *padapter) +{ + DBG_8723A("%s\n", __func__); +} + +static void update_bcn_wps_ie(struct rtw_adapter *padapter) +{ + u8 *pwps_ie = NULL, *pwps_ie_src, *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_8723A("%s\n", __func__); + + pwps_ie = rtw_get_wps_ie23a(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 = kmalloc(remainder_ielen, GFP_ATOMIC); + 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 rtw_adapter *padapter) +{ +} + +static void update_bcn_vendor_spec_ie(struct rtw_adapter *padapter, u8*oui) +{ + DBG_8723A("%s\n", __func__); + + if (!memcmp(RTW_WPA_OUI23A, oui, 4)) + { + update_bcn_wpa_ie(padapter); + } + else if (!memcmp(WMM_OUI23A, oui, 4)) + { + update_bcn_wmm_ie(padapter); + } + else if (!memcmp(WPS_OUI23A, oui, 4)) + { + update_bcn_wps_ie(padapter); + } + else if (!memcmp(P2P_OUI23A, oui, 4)) + { + update_bcn_p2p_ie(padapter); + } + else + { + DBG_8723A("unknown OUI type!\n"); + } +} + +void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx) +{ + struct mlme_priv *pmlmepriv; + struct mlme_ext_priv *pmlmeext; + /* struct mlme_ext_info *pmlmeinfo; */ + + /* DBG_8723A("%s\n", __func__); */ + + if (!padapter) + return; + + pmlmepriv = &padapter->mlmepriv; + pmlmeext = &padapter->mlmeextpriv; + /* pmlmeinfo = &pmlmeext->mlmext_info; */ + + if (false == 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_cmd23a(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 rtw_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 == true) + return 0; + + /* if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed) */ + /* return 0; */ + + DBG_8723A("%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 ((phtpriv_ap->ht_cap.cap_info & IEEE80211_HT_CAP_SUP_WIDTH_20_40) + && 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_8723A("%s new operation mode = 0x%X changes =%d\n", + __func__, pmlmepriv->ht_op_mode, op_mode_changes); + + return op_mode_changes; +} + +void associated_clients_update23a(struct rtw_adapter *padapter, u8 updated) +{ + /* update associcated stations cap. */ + if (updated == true) + { + struct list_head *phead, *plist, *ptmp; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + VCS_update23a(padapter, psta); + } + + spin_unlock_bh(&pstapriv->asoc_list_lock); + } +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void bss_cap_update_on_sta_join23a(struct rtw_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_beacon23a(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_beacon23a(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_beacon23a(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_beacon23a(padapter, _ERPINFO_IE_, NULL, true); + } + } + + } + + if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)) + { + 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_beacon23a(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_beacon23a(padapter, 0xFF, NULL, true); + } + } + } + + if (psta->flags & WLAN_STA_HT) + { + u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info); + + DBG_8723A("HT: STA " MAC_FMT " HT Capabilities " + "Info: 0x%04x\n", MAC_ARG(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_8723A("%s STA " MAC_FMT " - no " + "greenfield, num of non-gf stations %d\n", + __func__, MAC_ARG(psta->hwaddr), + pmlmepriv->num_sta_ht_no_gf); + } + + if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH_20_40) == 0) { + if (!psta->ht_20mhz_set) { + psta->ht_20mhz_set = 1; + pmlmepriv->num_sta_ht_20mhz++; + } + DBG_8723A("%s STA " MAC_FMT " - 20 MHz HT, " + "num of 20MHz HT STAs %d\n", + __func__, MAC_ARG(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 == true) { + DBG_8723A("%s STA " MAC_FMT + " - no HT, num of non-HT stations %d\n", + __func__, MAC_ARG(psta->hwaddr), + pmlmepriv->num_sta_no_ht); + } + } + + if (rtw_ht_operation_update(padapter) > 0) + { + update_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false); + update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true); + } + + /* update associcated stations cap. */ + associated_clients_update23a(padapter, beacon_updated); + + DBG_8723A("%s, updated =%d\n", __func__, beacon_updated); +} + +u8 bss_cap_update_on_sta_leave23a(struct rtw_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_beacon23a(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_beacon23a(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_beacon23a(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_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false); + update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true); + } + + /* update associcated stations cap. */ + + DBG_8723A("%s, updated =%d\n", __func__, beacon_updated); + + return beacon_updated; +} + +u8 ap_free_sta23a(struct rtw_adapter *padapter, struct sta_info *psta, bool active, u16 reason) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + u8 beacon_updated = false; + + if (!psta) + return beacon_updated; + + if (active == true) + { + /* tear down Rx AMPDU */ + send_delba23a(padapter, 0, psta->hwaddr);/* recipient */ + + /* tear down TX AMPDU */ + send_delba23a(padapter, 1, psta->hwaddr);/* originator */ + + issue_deauth23a(padapter, psta->hwaddr, reason); + } + + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + + /* report_del_sta_event23a(padapter, psta->hwaddr, reason); */ + + /* clear cam entry / key */ + /* clear_cam_entry23a(padapter, (psta->mac_id + 3)); */ + rtw_clearstakey_cmd23a(padapter, (u8*)psta, (u8)(psta->mac_id + 3), true); + + spin_lock_bh(&psta->lock); + psta->state &= ~_FW_LINKED; + spin_unlock_bh(&psta->lock); + + rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason); + + report_del_sta_event23a(padapter, psta->hwaddr, reason); + + beacon_updated = bss_cap_update_on_sta_leave23a(padapter, psta); + + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(padapter, psta); + spin_unlock_bh(&pstapriv->sta_hash_lock); + + return beacon_updated; +} + +int rtw_ap_inform_ch_switch23a (struct rtw_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_8723A(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; + + list_for_each(plist, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + issue_action_spct_ch_switch23a (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_switch23a (padapter, bc_addr, new_ch, ch_offset); + + return ret; +} + +int rtw_sta_flush23a(struct rtw_adapter *padapter) +{ + struct list_head *phead, *plist, *ptmp; + int ret = 0; + struct sta_info *psta; + 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}; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + + DBG_8723A(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; + + list_for_each_safe(plist, ptmp, phead) { + int stainfo_offset; + + psta = container_of(plist, struct sta_info, asoc_list); + + /* Remove sta from asoc_list */ + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + + /* Keep sta for ap_free_sta23a() beyond this asoc_list loop */ + stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) { + chk_alive_list[chk_alive_num++] = stainfo_offset; + } + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + /* For each sta in chk_alive_list, call ap_free_sta23a */ + for (i = 0; i < chk_alive_num; i++) { + psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]); + ap_free_sta23a(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING); + } + + issue_deauth23a(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING); + + associated_clients_update23a(padapter, true); + + return ret; +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void sta_info_update23a(struct rtw_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 == false) + psta->htpriv.ht_option = false; + + update_sta_info23a_apmode23a(padapter, psta); +} + +/* called >= TSR LEVEL for USB or SDIO Interface*/ +void ap_sta_info_defer_update23a(struct rtw_adapter *padapter, struct sta_info *psta) +{ + if (psta->state & _FW_LINKED) + { + /* add ratid */ + add_RATid23a(padapter, psta, 0);/* DM_RATR_STA_INIT */ + } +} + +/* restore hw setting from sw data structures */ +void rtw_ap_restore_network(struct rtw_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct sta_priv * pstapriv = &padapter->stapriv; + struct sta_info *psta; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct list_head *phead, *plist, *ptmp; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + + rtw_setopmode_cmd23a(padapter, Ndis802_11APMode); + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + start_bss_network(padapter, (u8*)&mlmepriv->cur_network.network); + + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) + { + /* restore group key, WEP keys is restored in ips_leave23a() */ + rtw_set_key23a(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0); + } + + /* per sta pairwise key and settings */ + if ((padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_) && + (padapter->securitypriv.dot11PrivacyAlgrthm != _AES_)) { + return; + } + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + + list_for_each_safe(plist, ptmp, phead) { + int stainfo_offset; + + psta = container_of(plist, struct sta_info, asoc_list); + + stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) { + chk_alive_list[chk_alive_num++] = stainfo_offset; + } + } + + spin_unlock_bh(&pstapriv->asoc_list_lock); + + for (i = 0; i < chk_alive_num; i++) { + psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]); + + if (psta == NULL) { + DBG_8723A(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter)); + } + else if (psta->state &_FW_LINKED) + { + Update_RA_Entry23a(padapter, psta); + /* pairwise key */ + rtw_setstakey_cmd23a(padapter, (unsigned char *)psta, true); + } + } +} + +void start_ap_mode23a(struct rtw_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; + + /* init_mlme_ap_info23a(padapter); */ + 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; ista_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_mode23a(struct rtw_adapter *padapter) +{ + struct list_head *phead, *plist, *ptmp; + 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 rtw_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_securitypriv23a */ + 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); + + list_for_each_safe(plist, ptmp, phead) { + paclnode = container_of(plist, struct rtw_wlan_acl_node, list); + + if (paclnode->valid == true) { + paclnode->valid = false; + + list_del_init(&paclnode->list); + + pacl_list->num--; + } + } + spin_unlock_bh(&pacl_node_q->lock); + + DBG_8723A("%s, free acl_node_queue, num =%d\n", __func__, pacl_list->num); + + rtw_sta_flush23a(padapter); + + /* free_assoc_sta_resources */ + rtw_free_all_stainfo23a(padapter); + + psta = rtw_get_bcmc_stainfo23a(padapter); + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(padapter, psta); + spin_unlock_bh(&pstapriv->sta_hash_lock); + + rtw_init_bcmc_stainfo23a(padapter); + + rtw23a_free_mlme_priv_ie_data(pmlmepriv); +} + +#endif /* CONFIG_8723AU_AP_MODE */ diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c new file mode 100644 index 000000000000..dbc6401c6e13 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_cmd.c @@ -0,0 +1,1874 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_CMD_C_ + +#include +#include +#include +#include +#include + +#ifdef CONFIG_8723AU_BT_COEXIST +#include +#endif /* CONFIG_8723AU_BT_COEXIST */ + +static struct cmd_hdl wlancmds[] = { + GEN_DRV_CMD_HANDLER(0, NULL) /*0*/ + GEN_DRV_CMD_HANDLER(0, NULL) + GEN_DRV_CMD_HANDLER(0, NULL) + GEN_DRV_CMD_HANDLER(0, NULL) + GEN_DRV_CMD_HANDLER(0, NULL) + GEN_DRV_CMD_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) /*10*/ + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), join_cmd_hdl23a) /*14*/ + GEN_MLME_EXT_HANDLER(sizeof (struct disconnect_parm), disconnect_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof (struct wlan_bssid_ex), createbss_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof (struct setopmode_parm), setopmode_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof (struct sitesurvey_parm), sitesurvey_cmd_hdl23a) /*18*/ + GEN_MLME_EXT_HANDLER(sizeof (struct setauth_parm), setauth_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof (struct setkey_parm), setkey_hdl23a) /*20*/ + GEN_MLME_EXT_HANDLER(sizeof (struct set_stakey_parm), set_stakey_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof (struct set_assocsta_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct del_assocsta_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct setstapwrstate_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct setbasicrate_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct getbasicrate_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct setdatarate_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct getdatarate_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct setphyinfo_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct getphyinfo_parm), NULL) /*30*/ + GEN_MLME_EXT_HANDLER(sizeof (struct setphy_parm), NULL) + GEN_MLME_EXT_HANDLER(sizeof (struct getphy_parm), NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) /*40*/ + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl23a) + GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl23a) /* 46 */ + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) /*50*/ + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(0, NULL) + GEN_MLME_EXT_HANDLER(sizeof(struct Tx_Beacon_param), tx_beacon_hdl23a) /*55*/ + + GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl23a) /*56*/ + GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl23a) /*57*/ + + GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl23a) /*58*/ + GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param), set_chplan_hdl23a) /*59*/ + GEN_MLME_EXT_HANDLER(sizeof(struct LedBlink_param), led_blink_hdl23a) /*60*/ + + GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param), set_csa_hdl23a) /*61*/ + GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param), tdls_hdl23a) /*62*/ +}; + +struct _cmd_callback rtw_cmd_callback[] = { + {GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/ + {GEN_CMD_CODE(_Write_MACREG), NULL}, + {GEN_CMD_CODE(_Read_BBREG), &rtw_getbbrfreg_cmdrsp_callback23a}, + {GEN_CMD_CODE(_Write_BBREG), NULL}, + {GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback23a}, + {GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/ + {GEN_CMD_CODE(_Read_EEPROM), NULL}, + {GEN_CMD_CODE(_Write_EEPROM), NULL}, + {GEN_CMD_CODE(_Read_EFUSE), NULL}, + {GEN_CMD_CODE(_Write_EFUSE), NULL}, + + {GEN_CMD_CODE(_Read_CAM), NULL}, /*10*/ + {GEN_CMD_CODE(_Write_CAM), NULL}, + {GEN_CMD_CODE(_setBCNITV), NULL}, + {GEN_CMD_CODE(_setMBIDCFG), NULL}, + {GEN_CMD_CODE(_JoinBss), &rtw_joinbss_cmd23a_callback}, /*14*/ + {GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd23a_callback}, /*15*/ + {GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd23a_callback}, + {GEN_CMD_CODE(_SetOpMode), NULL}, + {GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback23a}, /*18*/ + {GEN_CMD_CODE(_SetAuth), NULL}, + + {GEN_CMD_CODE(_SetKey), NULL}, /*20*/ + {GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback23a}, + {GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback23a}, + {GEN_CMD_CODE(_DelAssocSta), NULL}, + {GEN_CMD_CODE(_SetStaPwrState), NULL}, + {GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/ + {GEN_CMD_CODE(_GetBasicRate), NULL}, + {GEN_CMD_CODE(_SetDataRate), NULL}, + {GEN_CMD_CODE(_GetDataRate), NULL}, + {GEN_CMD_CODE(_SetPhyInfo), NULL}, + + {GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/ + {GEN_CMD_CODE(_SetPhy), NULL}, + {GEN_CMD_CODE(_GetPhy), NULL}, + {GEN_CMD_CODE(_readRssi), NULL}, + {GEN_CMD_CODE(_readGain), NULL}, + {GEN_CMD_CODE(_SetAtim), NULL}, /*35*/ + {GEN_CMD_CODE(_SetPwrMode), NULL}, + {GEN_CMD_CODE(_JoinbssRpt), NULL}, + {GEN_CMD_CODE(_SetRaTable), NULL}, + {GEN_CMD_CODE(_GetRaTable), NULL}, + + {GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/ + {GEN_CMD_CODE(_GetDTMReport), NULL}, + {GEN_CMD_CODE(_GetTXRateStatistics), NULL}, + {GEN_CMD_CODE(_SetUsbSuspend), NULL}, + {GEN_CMD_CODE(_SetH2cLbk), NULL}, + {GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/ + {GEN_CMD_CODE(_SetChannel), NULL}, /*46*/ + {GEN_CMD_CODE(_SetTxPower), NULL}, + {GEN_CMD_CODE(_SwitchAntenna), NULL}, + {GEN_CMD_CODE(_SetCrystalCap), NULL}, + {GEN_CMD_CODE(_SetSingleCarrierTx), NULL}, /*50*/ + + {GEN_CMD_CODE(_SetSingleToneTx), NULL}, /*51*/ + {GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL}, + {GEN_CMD_CODE(_SetContinuousTx), NULL}, + {GEN_CMD_CODE(_SwitchBandwidth), NULL}, /*54*/ + {GEN_CMD_CODE(_TX_Beacon), NULL},/*55*/ + + {GEN_CMD_CODE(_Set_MLME_EVT), NULL},/*56*/ + {GEN_CMD_CODE(_Set_Drv_Extra), NULL},/*57*/ + {GEN_CMD_CODE(_Set_H2C_MSG), NULL},/*58*/ + {GEN_CMD_CODE(_SetChannelPlan), NULL},/*59*/ + {GEN_CMD_CODE(_LedBlink), NULL},/*60*/ + + {GEN_CMD_CODE(_SetChannelSwitch), NULL},/*61*/ + {GEN_CMD_CODE(_TDLS), NULL},/*62*/ +}; + +/* +Caller and the rtw_cmd_thread23a can protect cmd_q by spin_lock. +No irqsave is necessary. +*/ + +int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv) +{ + int res = _SUCCESS; + + sema_init(&pcmdpriv->cmd_queue_sema, 0); + sema_init(&pcmdpriv->terminate_cmdthread_sema, 0); + + _rtw_init_queue23a(&pcmdpriv->cmd_queue); + + 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 - + ((unsigned long)(pcmdpriv->cmd_allocated_buf) & + (CMDBUFF_ALIGN_SZ - 1)); + + pcmdpriv->rsp_allocated_buf = kzalloc(MAX_RSPSZ + 4, GFP_KERNEL); + + if (!pcmdpriv->rsp_allocated_buf) { + res = _FAIL; + goto exit; + } + + pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 - + ((unsigned long)(pcmdpriv->rsp_allocated_buf) & 3); + + pcmdpriv->cmd_issued_cnt = 0; + pcmdpriv->cmd_done_cnt = 0; + pcmdpriv->rsp_cnt = 0; + +exit: + + return res; +} + +/* forward definition */ + +static void c2h_wk_callback(struct work_struct *work); +int _rtw_init_evt_priv23a(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_WORK(&pevtpriv->c2h_wk, c2h_wk_callback); + pevtpriv->c2h_wk_alive = false; + pevtpriv->c2h_queue = rtw_cbuf_alloc23a(C2H_QUEUE_MAX_LEN + 1); + + return res; +} + +void _rtw_free_evt_priv23a (struct evt_priv *pevtpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("+_rtw_free_evt_priv23a\n")); + cancel_work_sync(&pevtpriv->c2h_wk); + while(pevtpriv->c2h_wk_alive) + msleep(10); + + while (!rtw_cbuf_empty23a(pevtpriv->c2h_queue)) { + void *c2h; + if ((c2h = rtw_cbuf_pop23a(pevtpriv->c2h_queue)) != NULL && + c2h != (void *)pevtpriv) { + kfree(c2h); + } + } + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("-_rtw_free_evt_priv23a\n")); +} + +void _rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv) +{ + if (pcmdpriv) { + kfree(pcmdpriv->cmd_allocated_buf); + kfree(pcmdpriv->rsp_allocated_buf); + } +} + +/* +Calling Context: +rtw_enqueue_cmd23a 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_cmd23a(struct rtw_queue *queue, struct cmd_obj *obj) +{ + unsigned long irqL; + + if (obj == NULL) + goto exit; + + spin_lock_irqsave(&queue->lock, irqL); + + list_add_tail(&obj->list, &queue->queue); + + spin_unlock_irqrestore(&queue->lock, irqL); + +exit: + + return _SUCCESS; +} + +u32 rtw_init_evt_priv23a(struct evt_priv *pevtpriv) +{ + int res; + + res = _rtw_init_evt_priv23a(pevtpriv); + + return res; +} + +void rtw_free_evt_priv23a(struct evt_priv *pevtpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("rtw_free_evt_priv23a\n")); + _rtw_free_evt_priv23a(pevtpriv); +} + +void rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("rtw_free_cmd_priv23a\n")); + _rtw_free_cmd_priv23a(pcmdpriv); +} + +static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + /* set to true to allow enqueuing cmd when hw_init_completed is false */ + u8 bAllow = 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)) { + 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 == false && + bAllow == false) || pcmdpriv->cmdthd_running == false) + return _FAIL; + return _SUCCESS; +} + +u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ + int res = _FAIL; + struct rtw_adapter *padapter = pcmdpriv->padapter; + + if (!cmd_obj) + goto exit; + + cmd_obj->padapter = padapter; + + res = rtw_cmd_filter(pcmdpriv, cmd_obj); + if (res == _FAIL) { + rtw_free_cmd_obj23a(cmd_obj); + goto exit; + } + + res = _rtw_enqueue_cmd23a(&pcmdpriv->cmd_queue, cmd_obj); + + if (res == _SUCCESS) + up(&pcmdpriv->cmd_queue_sema); + +exit: + return res; +} + +static struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv) +{ + struct cmd_obj *obj; + struct rtw_queue *queue = &pcmdpriv->cmd_queue; + unsigned long irqL; + + spin_lock_irqsave(&queue->lock, irqL); + if (list_empty(&queue->queue)) + obj = NULL; + else { + obj = container_of((&queue->queue)->next, struct cmd_obj, list); + list_del_init(&obj->list); + } + + spin_unlock_irqrestore(&queue->lock, irqL); + + return obj; +} + +void rtw_cmd_clr_isr23a(struct cmd_priv *pcmdpriv) +{ + pcmdpriv->cmd_done_cnt++; +} + +void rtw_free_cmd_obj23a(struct cmd_obj *pcmd) +{ + + if (pcmd->cmdcode != _JoinBss_CMD_ && + pcmd->cmdcode != _CreateBss_CMD_) { + /* free parmbuf in cmd_obj */ + kfree(pcmd->parmbuf); + } + + if (pcmd->rsp) { + if (pcmd->rspsz != 0) { + /* free rsp in cmd_obj */ + kfree(pcmd->rsp); + } + } + + kfree(pcmd); +} + +int rtw_cmd_thread23a(void *context) +{ + u8 ret; + struct cmd_obj *pcmd; + u8 *pcmdbuf, *prspbuf; + u8 (*cmd_hdl)(struct rtw_adapter *padapter, u8* pbuf); + void (*pcmd_callback)(struct rtw_adapter *dev, struct cmd_obj *pcmd); + struct rtw_adapter *padapter = (struct rtw_adapter *)context; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + allow_signal(SIGTERM); + + pcmdbuf = pcmdpriv->cmd_buf; + prspbuf = pcmdpriv->rsp_buf; + + pcmdpriv->cmdthd_running = true; + up(&pcmdpriv->terminate_cmdthread_sema); + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("start r871x rtw_cmd_thread23a !!!!\n")); + + while(1) { + if (down_interruptible(&pcmdpriv->cmd_queue_sema)) + break; +_next: + if ((padapter->bDriverStopped == true) || + (padapter->bSurpriseRemoved == true)) { + DBG_8723A("%s: DriverStopped(%d) SurpriseRemoved(%d) " + "break at line %d\n", __func__, + padapter->bDriverStopped, + padapter->bSurpriseRemoved, __LINE__); + break; + } + + if (!(pcmd = rtw_dequeue_cmd(pcmdpriv))) + continue; + + if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) { + 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 < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) { + 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 < (sizeof(rtw_cmd_callback) / + sizeof(struct _cmd_callback))) { + pcmd_callback = + rtw_cmd_callback[pcmd->cmdcode].callback; + if (!pcmd_callback) { + 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_obj23a(pcmd); + } else { + /* todo: !!! fill rsp_buf to pcmd->rsp + if (pcmd->rsp!= NULL) */ + /* need conider that free cmd_obj in + rtw_cmd_callback */ + pcmd_callback(pcmd->padapter, pcmd); + } + } else { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("%s: cmdcode = 0x%x callback not defined!\n", + __func__, pcmd->cmdcode)); + rtw_free_cmd_obj23a(pcmd); + } + + if (signal_pending (current)) + flush_signals(current); + + goto _next; + + } + pcmdpriv->cmdthd_running = false; + + /* free all cmd_obj resources */ + do { + pcmd = rtw_dequeue_cmd(pcmdpriv); + if (!pcmd) + break; + + rtw_free_cmd_obj23a(pcmd); + } while(1); + + up(&pcmdpriv->terminate_cmdthread_sema); + + complete_and_exit(NULL, 0); +} + +u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter, + struct cfg80211_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_cmd23a(padapter, LPS_CTRL_SCAN, 1); + +#ifdef CONFIG_8723AU_P2P + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + p2p_ps_wk_cmd23a(padapter, P2P_PS_SCAN, 1); + } +#endif /* CONFIG_8723AU_P2P */ + + ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC); + if (!ph2c) + return _FAIL; + + psurveyPara = kzalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC); + if (!psurveyPara) { + kfree(ph2c); + return _FAIL; + } + + rtw_free_network_queue23a(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].ssid_len) { + memcpy(&psurveyPara->ssid[i], &ssid[i], + sizeof(struct cfg80211_ssid)); + psurveyPara->ssid_num++; + if (0) + DBG_8723A(FUNC_ADPT_FMT" ssid:(%s, %d)\n", + FUNC_ADPT_ARG(padapter), + psurveyPara->ssid[i].ssid, + psurveyPara->ssid[i].ssid_len); + } + } + } + + /* 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 & IEEE80211_CHAN_DISABLED)) { + memcpy(&psurveyPara->ch[i], &ch[i], + sizeof(struct rtw_ieee80211_channel)); + psurveyPara->ch_num++; + if (0) + DBG_8723A(FUNC_ADPT_FMT" ch:%u\n", + FUNC_ADPT_ARG(padapter), + psurveyPara->ch[i].hw_value); + } + } + } + + set_fwstate(pmlmepriv, _FW_UNDER_SURVEY); + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); + + if (res == _SUCCESS) { + mod_timer(&pmlmepriv->scan_to_timer, jiffies + + msecs_to_jiffies(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; +} + +void rtw_getbbrfreg_cmdrsp_callback23a(struct rtw_adapter *padapter, + struct cmd_obj *pcmd) +{ + kfree(pcmd->parmbuf); + kfree(pcmd); +} + +u8 rtw_createbss_cmd23a(struct rtw_adapter *padapter) +{ + struct cmd_obj *pcmd; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_bssid_ex *pdev_network; + u8 res = _SUCCESS; + + pdev_network = &padapter->registrypriv.dev_network; + + rtw_led_control(padapter, LED_CTL_START_TO_LINK); + + if (pmlmepriv->assoc_ssid.ssid_len == 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_ATOMIC); + if (!pcmd) { + 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_cmd23a(pcmdpriv, pcmd); + +exit: + + return res; +} + +u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter, + struct wlan_network * pnetwork) +{ + u8 *auth, 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_net_infra ndis_network_mode; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + ndis_network_mode = pnetwork->network.InfrastructureMode; + + rtw_led_control(padapter, LED_CTL_START_TO_LINK); + + if (pmlmepriv->assoc_ssid.ssid_len == 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) { + res = _FAIL; + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("rtw_joinbss_cmd23a: 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) { + if (pcmd) + kfree(pcmd); + + res = _FAIL; + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("rtw_joinbss_cmd23a :psecnetwork == NULL!!!\n")); + + goto exit; + } + + memset(psecnetwork, 0, t_len); + + memcpy(psecnetwork, &pnetwork->network, + get_wlan_bssid_ex_sz(&pnetwork->network)); + + auth = &psecuritypriv->authenticator_ie[0]; + 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 == false) + ether_addr_copy(&pmlmepriv->assoc_bssid[0], + &pnetwork->network.MacAddress[0]); + + psecnetwork->IELength = + rtw_restruct_sec_ie23a(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_ie23a(padapter, + &pnetwork->network.IEs[0], + &psecnetwork->IEs[0], + pnetwork->network.IELength, + psecnetwork->IELength); + + if (psecnetwork->IELength != tmp_len) { + psecnetwork->IELength = tmp_len; + /* There is WMM IE in this corresp. beacon */ + pqospriv->qos_option = 1; + } else { + /* There is no WMM IE in this corresp. beacon */ + pqospriv->qos_option = 0; + } + } + + 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_ie23a */ + rtw_restructure_ht_ie23a(padapter, + &pnetwork->network.IEs[0], + &psecnetwork->IEs[0], + pnetwork->network.IELength, + &psecnetwork->IELength); + } + } + + pmlmeinfo->assoc_AP_vendor = + check_assoc_AP23a(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_8723A("%s: smart_ps =%d\n", __func__, + padapter->pwrctrlpriv.smart_ps); + + /* get cmdsz before endian conversion */ + pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork); + + 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_cmd23a(pcmdpriv, pcmd); +exit: + + return res; +} + +u8 rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms, + bool enqueue) +{ + 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_cmd23a\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(struct cmd_obj), GFP_ATOMIC); + if (!cmdobj) { + res = _FAIL; + kfree(param); + goto exit; + } + init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_); + res = rtw_enqueue_cmd23a(cmdpriv, cmdobj); + } else { + /* no need to enqueue, do the cmd hdl directly and + free cmd parameter */ + if (H2C_SUCCESS != disconnect_hdl23a(padapter, (u8 *)param)) + res = _FAIL; + kfree(param); + } + +exit: + return res; +} + +u8 rtw_setopmode_cmd23a(struct rtw_adapter *padapter, + enum ndis_802_11_net_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) { + res = false; + goto exit; + } + psetop = kzalloc(sizeof(struct setopmode_parm), GFP_KERNEL); + + if (!psetop) { + kfree(ph2c); + res = false; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_); + psetop->mode = (u8)networktype; + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); +exit: + return res; +} + +u8 rtw_setstakey_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_KERNEL); + if (!psetstakey_para) { + kfree(ph2c); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_KERNEL); + if (!psetstakey_rsp) { + 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); + + ether_addr_copy(psetstakey_para->addr, sta->hwaddr); + + 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 == true) { + memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16); + } else { + int idx = psecuritypriv->dot118021XGrpKeyid; + memcpy(&psetstakey_para->key, + &psecuritypriv->dot118021XGrpKey[idx].skey, 16); + } + + /* jeff: set this becasue at least sw key is ready */ + padapter->securitypriv.busetkipkey = true; + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); + +exit: + + return res; +} + +u8 rtw_clearstakey_cmd23a(struct rtw_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_entry23a(padapter, entry); + } else { + ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), + GFP_KERNEL); + if (!psetstakey_para) { + kfree(ph2c); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), + GFP_KERNEL); + if (!psetstakey_rsp) { + 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); + + ether_addr_copy(psetstakey_para->addr, sta->hwaddr); + + psetstakey_para->algorithm = _NO_PRIVACY_; + + psetstakey_para->id = entry; + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); + } +exit: + return res; +} + +u8 rtw_addbareq_cmd23a(struct rtw_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_ATOMIC); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + paddbareq_parm = kzalloc(sizeof(struct addBaReq_parm), GFP_ATOMIC); + if (!paddbareq_parm) { + kfree(ph2c); + res = _FAIL; + goto exit; + } + + paddbareq_parm->tid = tid; + ether_addr_copy(paddbareq_parm->addr, addr); + + init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, + GEN_CMD_CODE(_AddBAReq)); + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); +exit: + return res; +} + +u8 rtw_dynamic_chk_wk_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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)); + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); +exit: + + return res; +} + +/* + * This is only ever called from on_action_spct23a_ch_switch () which isn't + * called from anywhere itself + */ +u8 rtw_set_ch_cmd23a(struct rtw_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_8723A(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_KERNEL); + if (!set_ch_parm) { + 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_KERNEL); + if (!pcmdobj) { + 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_cmd23a(pcmdpriv, pcmdobj); + } else { + /* no need to enqueue, do the cmd hdl directly and + free cmd parameter */ + if (H2C_SUCCESS != set_ch_hdl23a(padapter, (u8 *)set_ch_parm)) + res = _FAIL; + + kfree(set_ch_parm); + } + + /* do something based on res... */ +exit: + + DBG_8723A(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), + res); + + return res; +} + +static void traffic_status_watchdog(struct rtw_adapter *padapter) +{ + u8 bEnterPS; + u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false; + u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false; + u8 bHigherBusyTxTraffic = false; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + /* */ + /* Determine if our traffic is busy now */ + /* */ + if (check_fwstate(pmlmepriv, _FW_LINKED)) { +#ifdef CONFIG_8723AU_BT_COEXIST + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 50 || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 50) +#else /* !CONFIG_8723AU_BT_COEXIST */ + /* if we raise bBusyTraffic in last watchdog, using + lower threshold. */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic) + BusyThreshold = 75; + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) +#endif /* !CONFIG_8723AU_BT_COEXIST */ + { + 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; + } + +#ifdef CONFIG_8723AU_BT_COEXIST + if (BT_1Ant(padapter) == false) +#endif + { + /* 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_Enter23a(padapter); + else + LPS_Leave23a(padapter); + } + } else + LPS_Leave23a(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; +} + +void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz) +{ + struct mlme_priv *pmlmepriv; + + padapter = (struct rtw_adapter *)pbuf; + pmlmepriv = &padapter->mlmepriv; + +#ifdef CONFIG_8723AU_AP_MODE + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) + expire_timeout_chk23a(padapter); +#endif + + rtw_hal_sreset_xmit_status_check23a(padapter); + + linked_status_chk23a(padapter); + traffic_status_watchdog(padapter); + + rtw_hal_dm_watchdog23a(padapter); + +#ifdef CONFIG_8723AU_BT_COEXIST + /* */ + /* BT-Coexist */ + /* */ + BT_CoexistMechanism(padapter); +#endif +} + +void lps_ctrl_wk_hdl(struct rtw_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: +#ifdef CONFIG_8723AU_BT_COEXIST + BT_WifiScanNotify(padapter, true); + if (BT_1Ant(padapter) == false) +#endif + { + if (check_fwstate(pmlmepriv, _FW_LINKED)) + LPS_Leave23a(padapter); + } + break; + case LPS_CTRL_JOINBSS: + LPS_Leave23a(padapter); + break; + case LPS_CTRL_CONNECT: + mstatus = 1;/* connect */ + /* Reset LPS Setting */ + padapter->pwrctrlpriv.LpsIdleCount = 0; + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT, + (u8 *)&mstatus); +#ifdef CONFIG_8723AU_BT_COEXIST + BT_WifiMediaStatusNotify(padapter, mstatus); +#endif + break; + case LPS_CTRL_DISCONNECT: + mstatus = 0;/* disconnect */ +#ifdef CONFIG_8723AU_BT_COEXIST + BT_WifiMediaStatusNotify(padapter, mstatus); + if (BT_1Ant(padapter) == false) +#endif + { + LPS_Leave23a(padapter); + } + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT, + (u8 *)&mstatus); + break; + case LPS_CTRL_SPECIAL_PACKET: + pwrpriv->DelayLPSLastTimeStamp = jiffies; +#ifdef CONFIG_8723AU_BT_COEXIST + BT_SpecialPacketNotify(padapter); + if (BT_1Ant(padapter) == false) +#endif + { + LPS_Leave23a(padapter); + } + break; + case LPS_CTRL_LEAVE: +#ifdef CONFIG_8723AU_BT_COEXIST + BT_LpsLeave(padapter); + if (BT_1Ant(padapter) == false) +#endif + { + LPS_Leave23a(padapter); + } + break; + + default: + break; + } +} + +u8 rtw_lps_ctrl_wk_cmd23a(struct rtw_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; + u8 res = _SUCCESS; + + if (enqueue) { + ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), + GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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_cmd23a(pcmdpriv, ph2c); + } else + lps_ctrl_wk_hdl(padapter, lps_ctrl_type); +exit: + + return res; +} + +static void power_saving_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz) +{ + rtw_ps_processor23a(padapter); +} + +#ifdef CONFIG_8723AU_P2P +u8 p2p_protocol_wk_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), + GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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_cmd23a(pcmdpriv, ph2c); +exit: + + return res; +} +#endif /* CONFIG_8723AU_P2P */ + +u8 rtw_ps_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), + GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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_cmd23a(pcmdpriv, ppscmd); +exit: + + return res; +} + +#ifdef CONFIG_8723AU_AP_MODE + +static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter) +{ + int cnt = 0; + struct sta_info *psta_bmc; + struct sta_priv *pstapriv = &padapter->stapriv; + + psta_bmc = rtw_get_bcmc_stainfo23a(padapter); + if (!psta_bmc) + return; + + if (psta_bmc->sleepq_len == 0) { + u8 val = 0; + + rtw23a_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val); + + while(val == false) { + msleep(100); + + cnt++; + + if (cnt>10) + break; + + rtw23a_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_beacon23a(padapter, _TIM_IE_, NULL, false); + } else /* re check again */ + rtw_chk_hi_queue_cmd23a(padapter); + } +} + +u8 rtw_chk_hi_queue_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), + GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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_cmd23a(pcmdpriv, ph2c); +exit: + + return res; +} +#endif + +u8 rtw_c2h_wk_cmd23a(struct rtw_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) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), + GFP_ATOMIC); + if (!pdrvextra_cmd_parm) { + 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_cmd23a(pcmdpriv, ph2c); + +exit: + + return res; +} + +s32 c2h_evt_hdl(struct rtw_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_read23a(adapter, buf) == _SUCCESS) { + c2h_evt = (struct c2h_evt_hdr *)buf; + + if (filter && filter(c2h_evt->id) == false) + goto exit; + + ret = rtw_hal_c2h_handler23a(adapter, c2h_evt); + } + } else { + + if (filter && filter(c2h_evt->id) == false) + goto exit; + + ret = rtw_hal_c2h_handler23a(adapter, c2h_evt); + } +exit: + return ret; +} + +static void c2h_wk_callback(struct work_struct *work) +{ + struct evt_priv *evtpriv; + struct rtw_adapter *adapter; + struct c2h_evt_hdr *c2h_evt; + c2h_id_filter ccx_id_filter; + + evtpriv = container_of(work, struct evt_priv, c2h_wk); + adapter = container_of(evtpriv, struct rtw_adapter, evtpriv); + ccx_id_filter = rtw_hal_c2h_id_filter_ccx23a(adapter); + + evtpriv->c2h_wk_alive = true; + + while (!rtw_cbuf_empty23a(evtpriv->c2h_queue)) { + c2h_evt = (struct c2h_evt_hdr *) + rtw_cbuf_pop23a(evtpriv->c2h_queue); + if (c2h_evt) { + /* This C2H event is read, clear it */ + c2h_evt_clear23a(adapter); + } else if ((c2h_evt = (struct c2h_evt_hdr *) + kmalloc(16, GFP_ATOMIC))) { + /* This C2H event is not read, read & clear now */ + if (c2h_evt_read23a(adapter, (u8*)c2h_evt) != _SUCCESS) + continue; + } + + /* Special pointer to trigger c2h_evt_clear23a 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_handler23a(adapter, c2h_evt); + kfree(c2h_evt); + } else { + /* Enqueue into cmd_thread for others */ + rtw_c2h_wk_cmd23a(adapter, (u8 *)c2h_evt); + } + } + + evtpriv->c2h_wk_alive = false; +} + +u8 rtw_drvextra_cmd_hdl23a(struct rtw_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; +#ifdef CONFIG_8723AU_P2P + case P2P_PS_WK_CID: + p2p_ps_wk_hdl23a(padapter, pdrvextra_cmd->type_size); + break; +#endif /* CONFIG_8723AU_P2P */ + case P2P_PROTO_WK_CID: + /* Commented by Albert 2011/07/01 */ + /* I used the type_size as the type command */ + p2p_protocol_wk_hdl23a(padapter, pdrvextra_cmd->type_size); + break; +#ifdef CONFIG_8723AU_AP_MODE + case CHECK_HIQ_WK_CID: + rtw_chk_hi_queue_hdl(padapter); + break; +#endif /* CONFIG_8723AU_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); + pdrvextra_cmd->pbuf = NULL; + } + + return H2C_SUCCESS; +} + +void rtw_survey_cmd_callback23a(struct rtw_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 */ + mod_timer(&pmlmepriv->scan_to_timer, + jiffies + msecs_to_jiffies(1)); + } else if (pcmd->res != H2C_SUCCESS) { + mod_timer(&pmlmepriv->scan_to_timer, + jiffies + msecs_to_jiffies(1)); + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("\n ********Error: MgntActrtw_set_802_11_bssid23a_" + "LIST_SCAN Fail ************\n\n.")); + } + + /* free cmd */ + rtw_free_cmd_obj23a(pcmd); +} + +void rtw_disassoc_cmd23a_callback(struct rtw_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; + } + + /* free cmd */ + rtw_free_cmd_obj23a(pcmd); +} + +void rtw_joinbss_cmd23a_callback(struct rtw_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 */ + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(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")); + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(1)); + } + + rtw_free_cmd_obj23a(pcmd); +} + +void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter, + struct cmd_obj *pcmd) +{ + struct sta_info *psta; + struct wlan_network *pwlan; + 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_cmd23a_callback " + "Fail ************\n\n.")); + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(1)); + } + + del_timer_sync(&pmlmepriv->assoc_timer); + + spin_lock_bh(&pmlmepriv->lock); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + psta = rtw_get_stainfo23a(&padapter->stapriv, + pnetwork->MacAddress); + if (!psta) { + psta = rtw_alloc_stainfo23a(&padapter->stapriv, + pnetwork->MacAddress); + if (!psta) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("\nCan't alloc sta_info when " + "createbss_cmd_callback\n")); + goto createbss_cmd_fail ; + } + } + + rtw_indicate_connect23a(padapter); + } else { + pwlan = rtw_alloc_network(pmlmepriv); + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + if (!pwlan) { + pwlan = rtw_get_oldest_wlan_network23a(&pmlmepriv->scanned_queue); + if (!pwlan) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("\n Error: can't get pwlan in " + "rtw23a_joinbss_event_cb\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); + /* pwlan->fixed = true; */ + + /* list_add_tail(&pwlan->list, + &pmlmepriv->scanned_queue.queue); */ + + /* copy pdev_network information to + pmlmepriv->cur_network */ + memcpy(&tgt_network->network, pnetwork, + get_wlan_bssid_ex_sz(pnetwork)); + + /* reset DSConfig */ + + _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_callback23a) */ + } + +createbss_cmd_fail: + + spin_unlock_bh(&pmlmepriv->lock); + + rtw_free_cmd_obj23a(pcmd); +} + +void rtw_setstaKey_cmdrsp_callback23a(struct rtw_adapter *padapter, + struct cmd_obj *pcmd) +{ + struct sta_priv *pstapriv; + struct set_stakey_rsp *psetstakey_rsp; + struct sta_info *psta; + + pstapriv = &padapter->stapriv; + psetstakey_rsp = (struct set_stakey_rsp*) (pcmd->rsp); + psta = rtw_get_stainfo23a(pstapriv, psetstakey_rsp->addr); + + if (!psta) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, + ("\nERROR: rtw_setstaKey_cmdrsp_callback23a => " + "can't get sta_info\n\n")); + goto exit; + } + +exit: + + rtw_free_cmd_obj23a(pcmd); +} + +void rtw_setassocsta_cmdrsp_callback23a(struct rtw_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_rsp* passocsta_rsp; + struct sta_info *psta; + + passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf); + passocsta_rsp = (struct set_assocsta_rsp*) (pcmd->rsp); + psta = rtw_get_stainfo23a(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 = 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_obj23a(pcmd); +} + +void rtw_getrttbl_cmd_cmdrsp_callback(struct rtw_adapter *padapter, + struct cmd_obj *pcmd) +{ + rtw_free_cmd_obj23a(pcmd); +} diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c new file mode 100644 index 000000000000..94221ee45bd2 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_efuse.c @@ -0,0 +1,718 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_EFUSE_C_ + +#include +#include + +#include + +/*------------------------Define local variable------------------------------*/ + +/* */ +#define REG_EFUSE_CTRL 0x0030 +#define EFUSE_CTRL REG_EFUSE_CTRL /* E-Fuse Control. */ +/* */ + +/*----------------------------------------------------------------------------- + * Function: Efuse_PowerSwitch23a + * + * 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_PowerSwitch23a( + struct rtw_adapter * pAdapter, + u8 bWrite, + u8 PwrState) +{ + pAdapter->HalFunc.EfusePowerSwitch(pAdapter, bWrite, PwrState); +} + +/*----------------------------------------------------------------------------- + * Function: efuse_GetCurrentSize23a + * + * 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_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType) +{ + u16 ret = 0; + + ret = pAdapter->HalFunc.EfuseGetCurrentSize(pAdapter, efuseType); + + return ret; +} + +/* 11/16/2008 MH Add description. Get current efuse area enabled word!!. */ +u8 +Efuse_CalculateWordCnts23a(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. */ +/* Refered 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 +ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf) +{ + u32 value32; + u8 readbyte; + u16 retry; + /* u32 start = rtw_get_current_time(); */ + + /* 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<10)) */ + 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. */ + udelay(50); + value32 = rtw_read32(Adapter, EFUSE_CTRL); + + *pbuf = (u8)(value32 & 0xff); + /* DBG_8723A("ReadEFuseByte23a _offset:%08u, in %d ms\n", _offset , rtw_get_passing_time_ms23a(start)); */ +} + +/* */ +/* Description: */ +/* 1. Execute E-Fuse read byte operation according as map offset and */ +/* save to E-Fuse table. */ +/* 2. Refered 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. */ +/* */ + +void +efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType, + u16 _offset, u16 _size_byte, u8 *pbuf); +void +efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType, + u16 _offset, u16 _size_byte, u8 *pbuf) +{ + Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, + _size_byte, pbuf); +} + +void +EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType, + u8 type, void *pOut) +{ + pAdapter->HalFunc.EFUSEGetEfuseDefinition(pAdapter, efuseType, + type, pOut); +} + +/*----------------------------------------------------------------------------- + * Function: EFUSE_Read1Byte23a + * + * 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_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address) +{ + u8 data; + u8 Bytetemp = {0x00}; + u8 temp = {0x00}; + u32 k = 0; + u16 contentLen = 0; + + EFUSE_GetEfuseDefinition23a(Adapter, EFUSE_WIFI, + TYPE_EFUSE_REAL_CONTENT_LEN, + (void *)&contentLen); + + 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_Read1Byte23a */ + +/*----------------------------------------------------------------------------- + * Function: EFUSE_Write1Byte + * + * Overview: Copy from WMAC fot EFUSE write 1 byte. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 09/23/2008 MHC Copy from WMAC. + * + *---------------------------------------------------------------------------*/ + +void +EFUSE_Write1Byte( + struct rtw_adapter * Adapter, + u16 Address, + u8 Value); +void +EFUSE_Write1Byte( + struct rtw_adapter * Adapter, + u16 Address, + u8 Value) +{ + u8 Bytetemp = {0x00}; + u8 temp = {0x00}; + u32 k = 0; + u16 contentLen = 0; + + /* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr =%x Data =%x\n", Address, Value)); */ + EFUSE_GetEfuseDefinition23a(Adapter, EFUSE_WIFI, + TYPE_EFUSE_REAL_CONTENT_LEN, + (void *)&contentLen); + + if (Address < contentLen) /* E-fuse 512Byte */ + { + rtw_write8(Adapter, EFUSE_CTRL, Value); + + /* 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]= 1 */ + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + temp = Bytetemp | 0x80; + rtw_write8(Adapter, EFUSE_CTRL+3, temp); + + /* Wait Write-ready (0x30[31]= 0) */ + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + while(Bytetemp & 0x80) + { + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + k++; + if (k == 100) + { + k = 0; + break; + } + } + } +}/* EFUSE_Write1Byte */ + +/* 11/16/2008 MH Read one byte from real Efuse. */ +u8 +efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data) +{ + u8 tmpidx = 0; + u8 bResult; + + /* -----------------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); + bResult = true; + } else { + *data = 0xff; + bResult = false; + } + return bResult; +} + +/* 11/16/2008 MH Write one byte to reald Efuse. */ +u8 +efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data) +{ + u8 tmpidx = 0; + u8 bResult; + + /* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr = %x Data =%x\n", addr, data)); */ + + /* return 0; */ + + /* -----------------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) + { + bResult = true; + } + else + { + bResult = false; + } + + return bResult; +} + +int +Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data) +{ + int ret = 0; + + ret = pAdapter->HalFunc.Efuse_PgPacketRead23a(pAdapter, offset, data); + + return ret; +} + +int +Efuse_PgPacketWrite23a(struct rtw_adapter *pAdapter, u8 offset, + u8 word_en, u8 *data) +{ + int ret; + + ret = pAdapter->HalFunc.Efuse_PgPacketWrite23a(pAdapter, offset, + word_en, data); + + return ret; +} + +/*----------------------------------------------------------------------------- + * Function: efuse_WordEnableDataRead23a + * + * 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_WordEnableDataRead23a(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_WordEnableDataWrite23a(struct rtw_adapter *pAdapter, u16 efuse_addr, + u8 word_en, u8 *data) +{ + u8 ret = 0; + + ret = pAdapter->HalFunc.Efuse_WordEnableDataWrite23a(pAdapter, efuse_addr, + word_en, data); + + return ret; +} + +static u8 efuse_read8(struct rtw_adapter *padapter, u16 address, u8 *value) +{ + return efuse_OneByteRead23a(padapter, address, value); +} + +static u8 efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value) +{ + return efuse_OneByteWrite23a(padapter, address, *value); +} + +/* + * read/wirte raw efuse data + */ +u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, 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 rtw_adapter *, u16, u8*); + + EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI, + TYPE_EFUSE_REAL_CONTENT_LEN, + (void *)&real_content_len); + EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI, + TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, + (void *)&max_available_size); + + if (start_addr > real_content_len) + return _FAIL; + + if (true == bWrite) { + if ((start_addr + cnts) > max_available_size) + return _FAIL; + rw8 = &efuse_write8; + } else + rw8 = &efuse_read8; + + Efuse_PowerSwitch23a(padapter, bWrite, 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_PowerSwitch23a(padapter, bWrite, false); + + return res; +} +/* */ +u16 efuse_GetMaxSize23a(struct rtw_adapter *padapter) +{ + u16 max_size; + EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI, + TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, + (void *)&max_size); + return max_size; +} +/* */ +u8 efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size) +{ + Efuse_PowerSwitch23a(padapter, false, true); + *size = Efuse_GetCurrentSize23a(padapter, EFUSE_WIFI); + Efuse_PowerSwitch23a(padapter, false, false); + + return _SUCCESS; +} +/* */ +u8 rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI, + TYPE_EFUSE_MAP_LEN, (void *)&mapLen); + + if ((addr + cnts) > mapLen) + return _FAIL; + + Efuse_PowerSwitch23a(padapter, false, true); + + efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data); + + Efuse_PowerSwitch23a(padapter, false, false); + + return _SUCCESS; +} + +u8 rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition23a(padapter, EFUSE_BT, + TYPE_EFUSE_MAP_LEN, (void *)&mapLen); + + if ((addr + cnts) > mapLen) + return _FAIL; + + Efuse_PowerSwitch23a(padapter, false, true); + + efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data); + + Efuse_PowerSwitch23a(padapter, false, false); + + return _SUCCESS; +} + +/*----------------------------------------------------------------------------- + * 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. + * + *---------------------------------------------------------------------------*/ +void +Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType, u8 *Efuse); +void +Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType, u8 *Efuse) +{ + u16 mapLen = 0; + + Efuse_PowerSwitch23a(pAdapter, false, true); + + EFUSE_GetEfuseDefinition23a(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, + (void *)&mapLen); + + efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse); + + Efuse_PowerSwitch23a(pAdapter, false, false); +} + +/*----------------------------------------------------------------------------- + * 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 rtw_adapter * pAdapter, + u16 Offset, + u8 *Value) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter); + + *Value = pEEPROM->efuse_eeprom_data[Offset]; +} /* EFUSE_ShadowRead23a1Byte */ + +/* Read Two Bytes */ +static void +efuse_ShadowRead2Byte( + struct rtw_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_ShadowRead23a2Byte */ + +/* Read Four Bytes */ +static void +efuse_ShadowRead4Byte( + struct rtw_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_ShadowMapUpdate23a + * + * 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_ShadowMapUpdate23a(struct rtw_adapter *pAdapter, u8 efuseType) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter); + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition23a(pAdapter, efuseType, + TYPE_EFUSE_MAP_LEN, (void *)&mapLen); + + if (pEEPROM->bautoload_fail_flag == true) + memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen); + else + Efuse_ReadAllMap(pAdapter, efuseType, + pEEPROM->efuse_eeprom_data); + +}/* EFUSE_ShadowMapUpdate23a */ + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowRead23a + * + * 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_ShadowRead23a( + struct rtw_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_ShadowRead23a */ diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c new file mode 100644 index 000000000000..8287f447cbdd --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c @@ -0,0 +1,1861 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _IEEE80211_C + +#include +#include +#include +#include +#include +#include + +u8 RTW_WPA_OUI23A_TYPE[] = { 0x00, 0x50, 0xf2, 1 }; +u16 RTW_WPA_VERSION23A = 1; +u8 WPA_AUTH_KEY_MGMT_NONE23A[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X23A[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X23A[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_NONE23A[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_CIPHER_SUITE_WEP4023A[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_CIPHER_SUITE_TKIP23A[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_WRAP23A[] = { 0x00, 0x50, 0xf2, 3 }; +u8 WPA_CIPHER_SUITE_CCMP23A[] = { 0x00, 0x50, 0xf2, 4 }; +u8 WPA_CIPHER_SUITE_WEP10423A[] = { 0x00, 0x50, 0xf2, 5 }; + +u16 RSN_VERSION_BSD23A = 1; +u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X23A[] = { 0x00, 0x0f, 0xac, 1 }; +u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X23A[] = { 0x00, 0x0f, 0xac, 2 }; +u8 RSN_CIPHER_SUITE_NONE23A[] = { 0x00, 0x0f, 0xac, 0 }; +u8 RSN_CIPHER_SUITE_WEP4023A[] = { 0x00, 0x0f, 0xac, 1 }; +u8 RSN_CIPHER_SUITE_TKIP23A[] = { 0x00, 0x0f, 0xac, 2 }; +u8 RSN_CIPHER_SUITE_WRAP23A[] = { 0x00, 0x0f, 0xac, 3 }; +u8 RSN_CIPHER_SUITE_CCMP23A[] = { 0x00, 0x0f, 0xac, 4 }; +u8 RSN_CIPHER_SUITE_WEP10423A[] = { 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_value23a(u8 val) +{ + unsigned char dot11_rate_table[]= + {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; + + 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_included23a(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_included23a(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_type23a(unsigned char *rate, int ratelen, int channel) +{ + if (channel > 14) { + if ((rtw_is_cckrates_included23a(rate)) == true) + return WIRELESS_INVALID; + else + return WIRELESS_11A; + } else { /* could be pure B, pure G, or B/G */ + if ((rtw_is_cckratesonly_included23a(rate)) == true) + return WIRELESS_11B; + else if ((rtw_is_cckrates_included23a(rate)) == true) + return WIRELESS_11BG; + else + return WIRELESS_11G; + } +} + +u8 *rtw_set_fixed_ie23a(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_ie23a will update frame length */ +u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen) +{ + + *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_ie23a_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_ie23a(buf, WLAN_EID_CHANNEL_SWITCH, 3, ie_data, buf_len); +} + +inline u8 secondary_ch_offset_to_hal_ch_offset23a(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_offset23a(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_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len, + u8 secondary_ch_offset) +{ + return rtw_set_ie23a(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET, + 1, &secondary_ch_offset, buf_len); +} + +inline u8 *rtw_set_ie23a_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; + RTW_PUT_LE16((u8*)&ie_data[2], reason); + RTW_PUT_LE16((u8*)&ie_data[4], precedence); + + return rtw_set_ie23a(buf, 0x118, 6, ie_data, buf_len); +} + +/*---------------------------------------------------------------------------- +index: the information element id index, limit is the limit for search +-----------------------------------------------------------------------------*/ +u8 *rtw_get_ie23a(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_ie23a_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_ie23a_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_ie23a - 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_ie23a(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_ie23a_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_rate23a(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_len23a(u8 *rateset) +{ + uint i = 0; + + while(1) { + if ((rateset[i]) == 0) + break; + + if (i > 12) + break; + + i++; + } + + return i; +} + +int rtw_generate_ie23a(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 */ + /* BCN_INTERVAL; */ + *(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod); + sz += 2; + ie += 2; + + /* capability info */ + *(u16*)ie = 0; + + *(u16*)ie |= cpu_to_le16(cap_IBSS); + + if (pregistrypriv->preamble == PREAMBLE_SHORT) + *(u16*)ie |= cpu_to_le16(cap_ShortPremble); + + if (pdev_network->Privacy) + *(u16*)ie |= cpu_to_le16(cap_Privacy); + + sz += 2; + ie += 2; + + /* SSID */ + ie = rtw_set_ie23a(ie, _SSID_IE_, pdev_network->Ssid.ssid_len, + 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_rate23a(pdev_network->SupportedRates, wireless_mode) ; + + rateLen = rtw_get_rateset_len23a(pdev_network->SupportedRates); + + if (rateLen > 8) { + ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, 8, + pdev_network->SupportedRates, &sz); + /* ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */ + } else { + ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, rateLen, + pdev_network->SupportedRates, &sz); + } + + /* DS parameter set */ + ie = rtw_set_ie23a(ie, _DSSET_IE_, 1, + (u8 *)&pdev_network->Configuration.DSConfig, &sz); + + /* IBSS Parameter Set */ + + ie = rtw_set_ie23a(ie, _IBSS_PARA_IE_, 2, + (u8 *)&pdev_network->Configuration.ATIMWindow, &sz); + + if (rateLen > 8) { + ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), + (pdev_network->SupportedRates + 8), &sz); + } + + + + /* return _SUCCESS; */ + + return sz; +} + +unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit) +{ + int len; + u16 val16; + unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01}; + u8 *pbuf = pie; + int limit_new = limit; + + while(1) { + pbuf = rtw_get_ie23a(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 *)&val16, (pbuf + 6), sizeof(val16)); + + val16 = le16_to_cpu(val16); + 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_ie23a(unsigned char *pie, int *rsn_ie_len, int limit) +{ + return rtw_get_ie23a(pie, _WPA2_IE_ID_, rsn_ie_len, limit); +} + +int rtw_get_wpa_cipher_suite23a(u8 *s) +{ + if (!memcmp(s, WPA_CIPHER_SUITE_NONE23A, WPA_SELECTOR_LEN)) + return WPA_CIPHER_NONE; + if (!memcmp(s, WPA_CIPHER_SUITE_WEP4023A, WPA_SELECTOR_LEN)) + return WPA_CIPHER_WEP40; + if (!memcmp(s, WPA_CIPHER_SUITE_TKIP23A, WPA_SELECTOR_LEN)) + return WPA_CIPHER_TKIP; + if (!memcmp(s, WPA_CIPHER_SUITE_CCMP23A, WPA_SELECTOR_LEN)) + return WPA_CIPHER_CCMP; + if (!memcmp(s, WPA_CIPHER_SUITE_WEP10423A, WPA_SELECTOR_LEN)) + return WPA_CIPHER_WEP104; + + return 0; +} + +int rtw_get_wpa2_cipher_suite23a(u8 *s) +{ + if (!memcmp(s, RSN_CIPHER_SUITE_NONE23A, RSN_SELECTOR_LEN)) + return WPA_CIPHER_NONE; + if (!memcmp(s, RSN_CIPHER_SUITE_WEP4023A, RSN_SELECTOR_LEN)) + return WPA_CIPHER_WEP40; + if (!memcmp(s, RSN_CIPHER_SUITE_TKIP23A, RSN_SELECTOR_LEN)) + return WPA_CIPHER_TKIP; + if (!memcmp(s, RSN_CIPHER_SUITE_CCMP23A, RSN_SELECTOR_LEN)) + return WPA_CIPHER_CCMP; + if (!memcmp(s, RSN_CIPHER_SUITE_WEP10423A, RSN_SELECTOR_LEN)) + return WPA_CIPHER_WEP104; + + return 0; +} + +int rtw_parse_wpa_ie23a(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_OUI23A_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_suite23a(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 = le16_to_cpu(*(u16*)pos); */ + count = RTW_GET_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_suite23a(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_ie23a(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_suite23a(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 = le16_to_cpu(*(u16*)pos); */ + count = RTW_GET_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_suite23a(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_ie23a(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_ie23a: 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 = 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 = 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_ie23a(u8 *ie_ptr, uint *wps_ielen) +{ + u8 match = false; + u8 eid, wps_oui[4]= {0x0, 0x50, 0xf2, 0x04}; + + if (!ie_ptr) + return match; + + eid = ie_ptr[0]; + + if ((eid == _WPA_IE_ID_) && !memcmp(&ie_ptr[2], wps_oui, 4)) { + /* DBG_8723A("==> found WPS_IE.....\n"); */ + *wps_ielen = ie_ptr[1] + 2; + match = true; + } + return match; +} + +/** + * rtw_get_wps_ie23a - 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_ie23a(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_attr23a - 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_attr23a(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; + + /* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */ + 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_content23a - 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_content23a(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_attr23a(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_8723A("short vendor specific " + "information element ignored (len =%lu)\n", + (unsigned long) elen); + } + return -1; + } + + oui = RTW_GET_BE24(pos); + switch (oui) { + case WLAN_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_8723A("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_8723A("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_8723A("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_8723A("Unknown Broadcom " + "information element ignored " + "(type =%d len =%lu)\n", + pos[3], (unsigned long) elen); + return -1; + } + break; + + default: + DBG_8723A("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_elems23a(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_8723A("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_CAPABILITY: + 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_8723A("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; +} + +static 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_2char2num23a(u8 hch, u8 lch) +{ + return (key_char2num(hch) * 10) + key_char2num(lch); +} + +u8 key_2char2num23a(u8 hch, u8 lch) +{ + return (key_char2num(hch) << 4) | key_char2num(lch); +} + +void rtw_macaddr_cfg23a(u8 *mac_addr) +{ + u8 mac[ETH_ALEN]; + if (!mac_addr) + return; + + memcpy(mac, mac_addr, ETH_ALEN); + + if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac)) { + 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_8723A("MAC Address from efuse error, assign default " + "one !!!\n"); + } + DBG_8723A("rtw_macaddr_cfg23a MAC Address = "MAC_FMT"\n", + MAC_ARG(mac_addr)); +} + +void dump_ies23a(u8 *buf, u32 buf_len) { + u8* pos = (u8*)buf; + u8 id, len; + + while (pos-buf <= buf_len) { + id = *pos; + len = *(pos + 1); + + DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len); +#ifdef CONFIG_8723AU_P2P + dump_p2p_ie23a(pos, len); +#endif + dump_wps_ie23a(pos, len); + + pos += (2 + len); + } +} + +void dump_wps_ie23a(u8 *ie, u32 ie_len) { + u8* pos = (u8*)ie; + u16 id; + u16 len; + + u8 *wps_ie; + uint wps_ielen; + + wps_ie = rtw_get_wps_ie23a(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_8723A("%s ID:0x%04x, LEN:%u\n", __func__, id, len); + + pos += (4 + len); + } +} + +#ifdef CONFIG_8723AU_P2P +void dump_p2p_ie23a(u8 *ie, u32 ie_len) { + u8* pos = (u8*)ie; + u8 id; + u16 len; + + u8 *p2p_ie; + uint p2p_ielen; + + p2p_ie = rtw_get_p2p_ie23a(ie, ie_len, NULL, &p2p_ielen); + if (p2p_ie != ie || p2p_ielen == 0) + return; + + pos += 6; + while (pos-ie < ie_len) { + id = *pos; + len = RTW_GET_LE16(pos+1); + + DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len); + + pos+= (3+len); + } +} + +/** + * rtw_get_p2p_ie23a - 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_ie23a(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) + *p2p_ielen = 0; + + while (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; + + break; + } else { + cnt += in_ie[cnt + 1] + 2; /* goto next */ + } + } + + return NULL; +} + +/** + * rtw_get_p2p_attr23a - 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_attr23a(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 = RTW_GET_LE16(attr_ptr + 1); + u16 attr_len = attr_data_len + 3; + + /* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */ + 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_attr23a_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_attr23a_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_attr23a(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_content23a(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_attr23a(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); + /* dump_ies23a(ie, ielen); */ + + 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 { + /* if (index>0) */ + /* dump_ies23a(ie, ielen); */ + break; + } + } + + return ielen; +} + +void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id) +{ + u8 *p2p_ie; + uint p2p_ielen, p2p_ielen_ori; + + if ((p2p_ie = rtw_get_p2p_ie23a(bss_ex->IEs + _FIXED_IE_LENGTH_, + bss_ex->IELength - _FIXED_IE_LENGTH_, + NULL, &p2p_ielen_ori))) { + 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; + 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_8723AU_P2P */ + +#ifdef CONFIG_8723AU_P2P +int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen) +{ + int match; + uint cnt = 0; + u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A}; + + match = false; + + if (in_len < 0) { + return match; + } + + while (cnt < in_len) + { + eid = in_ie[cnt]; + + if ((eid == _VENDOR_SPECIFIC_IE_) && + !memcmp(&in_ie[cnt+2], wfd_oui, 4)) { + if (wfd_ie != NULL) { + memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + } else { + if (wfd_ielen != NULL) { + *wfd_ielen = 0; + } + } + + if (wfd_ielen != NULL) { + *wfd_ielen = in_ie[cnt + 1] + 2; + } + + cnt += in_ie[cnt + 1] + 2; + + match = true; + break; + } else { + cnt += in_ie[cnt + 1] +2; /* goto next */ + } + } + + if (match == true) { + match = cnt; + } + + return match; +} + +/* attr_content: The output buffer, contains the "body field" of + WFD attribute. */ +/* attr_contentlen: The data length of the "body field" of WFD + attribute. */ +int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, + u8 *attr_content, uint *attr_contentlen) +{ + int match; + uint cnt = 0; + u8 attr_id, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A}; + + match = false; + + if ((wfd_ie[0] != _VENDOR_SPECIFIC_IE_) || + memcmp(wfd_ie + 2, wfd_oui, 4)) { + return match; + } + + /* 1 (WFD IE) + 1 (Length) + 3 (OUI) + 1 (OUI Type) */ + cnt = 6; + while (cnt < wfd_ielen) { + u16 attrlen = RTW_GET_BE16(wfd_ie + cnt + 1); + + attr_id = wfd_ie[cnt]; + if (attr_id == target_attr_id) { + /* 3 -> 1 byte for attribute ID field, 2 + bytes for length field */ + if (attr_content) + memcpy(attr_content, &wfd_ie[cnt + 3], attrlen); + + if (attr_contentlen) + *attr_contentlen = attrlen; + + cnt += attrlen + 3; + + match = true; + break; + } else { + cnt += attrlen + 3; /* goto next */ + } + } + + return match; +} +#endif /* CONFIG_8723AU_P2P */ + +/* Baron adds to avoid FreeBSD warning */ +int ieee80211_is_empty_essid23a(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; +} + +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; + int r; + pbuf = rtw_get_wpa_ie23a(&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)); + r = rtw_parse_wpa_ie23a(pbuf, wpa_ielen + 2, &group_cipher, + &pairwise_cipher, &is8021x); + if (r == _SUCCESS) { + 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_ie23a(&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")); + r = rtw_parse_wpa2_ie23a(pbuf, wpa_ielen + 2, + &group_cipher, &pairwise_cipher, + &is8021x); + if (r == _SUCCESS) { + 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_info23a(struct wlan_network *pnetwork) +{ + unsigned short cap = 0; + u8 bencrypt = 0; + /* u8 wpa_ie[255], rsn_ie[255]; */ + 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(&cap, rtw_get_capability23a_from_ie(pnetwork->network.IEs), 2); + cap = le16_to_cpu(cap); + if (cap & WLAN_CAPABILITY_PRIVACY) { + bencrypt = 1; + pnetwork->network.Privacy = 1; + } else { + pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS; + } + rtw_get_sec_ie23a(pnetwork->network.IEs, pnetwork->network.IELength, + NULL, &rsn_len, NULL, &wpa_len); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("rtw_get_bcn_info23a: ssid =%s\n", pnetwork->network.Ssid.ssid)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("rtw_get_bcn_info23a: wpa_len =%d rsn_len =%d\n", + wpa_len, rsn_len)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("rtw_get_bcn_info23a: ssid =%s\n", pnetwork->network.Ssid.ssid)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("rtw_get_bcn_info23a: 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_info23a: pnetwork->encryp_protocol is %x\n", + pnetwork->BcnInfo.encryp_protocol)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("rtw_get_bcn_info23a: 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_ie23a(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 = pht_cap->cap_info; + } else { + pnetwork->BcnInfo.ht_cap_info = 0; + } + /* parsing HT_INFO_IE */ + p = rtw_get_ie23a(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_rate23a(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_parse23a(const u8 *frame, u32 frame_len, u8* category, + u8 *action) +{ + const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr); + u16 fc; + u8 c, a = 0; + + fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control); + + if ((fc & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) != + (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)) { + return false; + } + + c = frame_body[0]; + + switch (c) { + case WLAN_CATEGORY_VENDOR_SPECIFIC: /* vendor-specific */ + break; + default: + a = frame_body[1]; + } + + if (category) + *category = c; + if (action) + *action = a; + + return true; +} + +static const char *_action_public_str23a[] = { + "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_str23a(u8 action) +{ + action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action; + return _action_public_str23a[action]; +} diff --git a/drivers/staging/rtl8723au/core/rtw_io.c b/drivers/staging/rtl8723au/core/rtw_io.c new file mode 100644 index 000000000000..1cae8d7659b9 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_io.c @@ -0,0 +1,266 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +/* + +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: + +1. For 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 +#include +#include +#include + +#include + +u8 _rtw_read823a(struct rtw_adapter *adapter, u32 addr) +{ + u8 r_val; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + r_val = pintfhdl->io_ops._read8(pintfhdl, addr); + + return r_val; +} + +u16 _rtw_read1623a(struct rtw_adapter *adapter, u32 addr) +{ + u16 r_val; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + r_val = pintfhdl->io_ops._read16(pintfhdl, addr); + + return le16_to_cpu(r_val); +} + +u32 _rtw_read3223a(struct rtw_adapter *adapter, u32 addr) +{ + u32 r_val; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + r_val = pintfhdl->io_ops._read32(pintfhdl, addr); + + return le32_to_cpu(r_val); +} + +int _rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + ret = pintfhdl->io_ops._write8(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} + +int _rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + val = cpu_to_le16(val); + ret = pintfhdl->io_ops._write16(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} +int _rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + val = cpu_to_le32(val); + ret = pintfhdl->io_ops._write32(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} + +int _rtw_writeN23a(struct rtw_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 ret; + + ret = pintfhdl->io_ops._writeN(pintfhdl, addr, length, pdata); + + return RTW_STATUS_CODE23a(ret); +} +int _rtw_write823a_async23a(struct rtw_adapter *adapter, u32 addr, u8 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + ret = pintfhdl->io_ops._write8_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} +int _rtw_write1623a_async(struct rtw_adapter *adapter, u32 addr, u16 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + val = cpu_to_le16(val); + ret = pintfhdl->io_ops._write16_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} +int _rtw_write3223a_async23a(struct rtw_adapter *adapter, u32 addr, u32 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + int ret; + + val = cpu_to_le32(val); + ret = pintfhdl->io_ops._write32_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE23a(ret); +} + +void _rtw_read_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + if ((adapter->bDriverStopped == true) || + (adapter->bSurpriseRemoved == true)) { + RT_TRACE(_module_rtl871x_io_c_, _drv_info_, + ("rtw_read_mem:bDriverStopped(%d) OR " + "bSurpriseRemoved(%d)", adapter->bDriverStopped, + adapter->bSurpriseRemoved)); + return; + } + + pintfhdl->io_ops._read_mem(pintfhdl, addr, cnt, pmem); +} + +void _rtw_write_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + pintfhdl->io_ops._write_mem(pintfhdl, addr, cnt, pmem); +} + +void _rtw_read_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, + struct recv_buf *rbuf) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + + if ((adapter->bDriverStopped == true) || + (adapter->bSurpriseRemoved == true)) { + RT_TRACE(_module_rtl871x_io_c_, _drv_info_, + ("rtw_read_port:bDriverStopped(%d) OR " + "bSurpriseRemoved(%d)", adapter->bDriverStopped, + adapter->bSurpriseRemoved)); + return; + } + + pintfhdl->io_ops._read_port(pintfhdl, addr, cnt, rbuf); +} + +void _rtw_read_port23a_cancel(struct rtw_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_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, + struct xmit_buf *xbuf) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &pio_priv->intf; + u32 ret = _SUCCESS; + + ret = pintfhdl->io_ops._write_port(pintfhdl, addr, cnt, xbuf); + + return ret; +} + +u32 _rtw_write_port23a_and_wait23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, + struct xmit_buf *pxmitbuf, int timeout_ms) +{ + int ret = _SUCCESS; + struct submit_ctx sctx; + + rtw_sctx_init23a(&sctx, timeout_ms); + pxmitbuf->sctx = &sctx; + + ret = _rtw_write_port23a(adapter, addr, cnt, pxmitbuf); + + if (ret == _SUCCESS) + ret = rtw_sctx_wait23a(&sctx); + + return ret; +} + +void _rtw_write_port23a_cancel(struct rtw_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_priv23a(struct rtw_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/rtl8723au/core/rtw_ioctl_set.c b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c new file mode 100644 index 000000000000..30d7185e5637 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c @@ -0,0 +1,601 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_IOCTL_SET_C_ + +#include +#include +#include +#include + +#include +#include +#include + +u8 rtw_do_join23a(struct rtw_adapter *padapter) +{ + struct list_head *plist, *phead; + u8* pibss = NULL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct rtw_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_join23a: phead = %p; plist = %p\n\n\n", + phead, plist)); + + pmlmepriv->cur_network.join_res = -2; + + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + + pmlmepriv->to_join = true; + + if (_rtw_queue_empty23a(queue) == true) { + spin_unlock_bh(&pmlmepriv->scanned_queue.lock); + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + + /* when set_ssid/set_bssid for rtw_do_join23a(), but + scanning queue is empty */ + /* we try to issue sitesurvey firstly */ + + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false || + rtw_to_roaming(padapter) > 0) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("rtw_do_join23a(): site survey if scanned_queue " + "is empty\n.")); + /* submit site_survey23a_cmd */ + ret = rtw_sitesurvey_cmd23a(padapter, + &pmlmepriv->assoc_ssid, 1, + NULL, 0); + if (ret != _SUCCESS) { + pmlmepriv->to_join = false; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("rtw_do_join23a(): 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_queue23a(pmlmepriv); + if (select_ret == _SUCCESS) { + pmlmepriv->to_join = false; + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT)); + } else { + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) { + struct wlan_bssid_ex *pdev_network; + /* submit createbss_cmd to change to a + ADHOC_MASTER */ + + /* pmlmepriv->lock has been acquired by + caller... */ + pdev_network = + &padapter->registrypriv.dev_network; + + pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE; + + pibss = padapter->registrypriv.dev_network.MacAddress; + + memcpy(&pdev_network->Ssid, + &pmlmepriv->assoc_ssid, + sizeof(struct cfg80211_ssid)); + + rtw_update_registrypriv_dev_network23a(padapter); + + rtw_generate_random_ibss23a(pibss); + + if (rtw_createbss_cmd23a(padapter) != _SUCCESS) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, + _drv_err_, + ("***Error =>do_goin: rtw_creat" + "ebss_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_join23a(), + but there are no desired bss in scanning + queue */ + /* we try to issue sitesurvey firstly */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == + false || rtw_to_roaming(padapter) > 0) { + /* DBG_8723A("rtw_do_join23a() when no " + "desired bss in scanning queue\n"); + */ + ret = rtw_sitesurvey_cmd23a(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0); + if (ret != _SUCCESS) { + 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_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ssid) +{ + u8 status = _SUCCESS; + u32 cur_time = 0; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *pnetwork = &pmlmepriv->cur_network; + + + + DBG_8723A_LEVEL(_drv_always_, "set ssid [%s] fw_state = 0x%08x\n", + ssid->ssid, get_fwstate(pmlmepriv)); + + if (padapter->hw_init_completed == false) { + 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_8723A("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) == true) + { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n")); + + if ((pmlmepriv->assoc_ssid.ssid_len == ssid->ssid_len) && + !memcmp(&pmlmepriv->assoc_ssid.ssid, ssid->ssid, + ssid->ssid_len)) { + 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_ibss23a(padapter, pnetwork) == false) + { + /* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */ + rtw_disassoc_cmd23a(padapter, 0, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + rtw_indicate_disconnect23a(padapter); + + rtw_free_assoc_resources23a(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_cmd23a(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->ssid_len)); + 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.ssid_len)); + + rtw_disassoc_cmd23a(padapter, 0, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + rtw_indicate_disconnect23a(padapter); + + rtw_free_assoc_resources23a(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 == true) { + 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 cfg80211_ssid)); + pmlmepriv->assoc_by_bssid = false; + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) { + pmlmepriv->to_join = true; + } + else { + status = rtw_do_join23a(padapter); + } + +release_mlme_lock: + spin_unlock_bh(&pmlmepriv->lock); + +exit: + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("-rtw_set_802_11_ssid23a: status =%d\n", status)); + + + + return status; +} + +u8 rtw_set_802_11_infrastructure_mode23a(struct rtw_adapter* padapter, + enum ndis_802_11_net_infra networktype) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *cur_network = &pmlmepriv->cur_network; + enum ndis_802_11_net_infra* pold_state = &cur_network->network.InfrastructureMode; + + + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_, + ("+rtw_set_802_11_infrastructure_mode23a: 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_8723A("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_8723AU_AP_MODE + stop_ap_mode23a(padapter); +#endif + } + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||(*pold_state == Ndis802_11IBSS)) + rtw_disassoc_cmd23a(padapter, 0, true); + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) + rtw_free_assoc_resources23a(padapter, 1); + + if ((*pold_state == Ndis802_11Infrastructure) ||(*pold_state == Ndis802_11IBSS)) + { + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + rtw_indicate_disconnect23a(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_8723AU_AP_MODE + start_ap_mode23a(padapter); + /* rtw_indicate_connect23a(padapter); */ +#endif + + break; + + case Ndis802_11AutoUnknown: + case Ndis802_11InfrastructureMax: + break; + } + + /* SecClearAllKeys(adapter); */ + + /* RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n", */ + /* get_fwstate(pmlmepriv))); */ + + spin_unlock_bh(&pmlmepriv->lock); + } + + + + return true; +} + +u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter, + struct cfg80211_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_bssid23a_list_scan(), fw_state =%x\n", + get_fwstate(pmlmepriv))); + + if (!padapter) { + res = false; + goto exit; + } + if (padapter->hw_init_completed == false) { + res = false; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("\n === rtw_set_802_11_bssid23a_list_scan:" + "hw_init_completed == false ===\n")); + goto exit; + } + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING) || + (pmlmepriv->LinkDetectInfo.bBusyTraffic == true)) { + /* Scan or linking is in progress, do nothing. */ + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("rtw_set_802_11_bssid23a_list_scan fail since fw_state " + "= %x\n", get_fwstate(pmlmepriv))); + res = true; + + if (check_fwstate(pmlmepriv, + (_FW_UNDER_SURVEY|_FW_UNDER_LINKING))) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n")); + } else { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("\n###pmlmepriv->sitesurveyctrl.traffic_" + "busy == true\n")); + } + } else { + if (rtw_is_scan_deny(padapter)) { + DBG_8723A(FUNC_ADPT_FMT": scan deny\n", + FUNC_ADPT_ARG(padapter)); + return _SUCCESS; + } + + spin_lock_bh(&pmlmepriv->lock); + + res = rtw_sitesurvey_cmd23a(padapter, pssid, ssid_max_num, + NULL, 0); + + spin_unlock_bh(&pmlmepriv->lock); + } +exit: + return res; +} + +u8 rtw_set_802_11_authentication_mode23a(struct rtw_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_mode23a:" + "psecuritypriv->ndisauthtype =%d", + psecuritypriv->ndisauthtype)); + + if (psecuritypriv->ndisauthtype > 3) + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + + res = rtw_set_auth23a(padapter, psecuritypriv); + + if (res == _SUCCESS) + ret = true; + else + ret = false; + + return ret; +} + +u8 rtw_set_802_11_add_wep23a(struct rtw_adapter* padapter, + struct ndis_802_11_wep *wep) +{ + u8 bdefaultkey; + u8 btransmitkey; + int keyid, res; + struct security_priv *psecuritypriv = &padapter->securitypriv; + u8 ret = _SUCCESS; + + bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true; + btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true : false; + keyid = wep->KeyIndex & 0x3fffffff; + + if (keyid >= 4) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("MgntActrtw_set_802_11_add_wep23a: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_wep23a:wep->KeyLength = 5\n")); + break; + case 13: + psecuritypriv->dot11PrivacyAlgrthm = _WEP104_; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength = 13\n")); + break; + default: + psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength!= 5 " + "or 13\n")); + break; + } + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("rtw_set_802_11_add_wep23a: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_wep23a: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_key23a(padapter, psecuritypriv, keyid, 1); + + if (res == _FAIL) + ret = false; +exit: + + return ret; +} + +/* +* rtw_get_cur_max_rate23a - +* @adapter: pointer to _adapter structure +* +* Return 0 or 100Kbps +*/ +u16 rtw_get_cur_max_rate23a(struct rtw_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 (!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_ie23a(&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, 2); + + /* bw_40MHz = (pht_capie->cap_info& + IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1:0; */ + /* 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 = (pht_capie->cap_info & (IEEE80211_HT_CAP + _SGI_20|IEEE80211_HT_CAP_SGI_40)) ? 1 : 0; */ + short_GI_20 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_20) ? 1:0; + short_GI_40 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_40) ? 1:0; + + rtw23a_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, + (u8 *)(&rf_type)); + max_rate = rtw_mcs_rate23a(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; +} diff --git a/drivers/staging/rtl8723au/core/rtw_led.c b/drivers/staging/rtl8723au/core/rtw_led.c new file mode 100644 index 000000000000..68532a3b2c14 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_led.c @@ -0,0 +1,1899 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ + +#include +#include + +/* */ +/* Description: */ +/* Callback function of LED BlinkTimer, */ +/* it just schedules to corresponding BlinkWorkItem/led_blink_hdl23a */ +/* */ +static void BlinkTimerCallback(unsigned long data) +{ + struct led_8723a *pLed = (struct led_8723a *)data; + struct rtw_adapter *padapter = pLed->padapter; + + /* DBG_8723A("%s\n", __func__); */ + + if ((padapter->bSurpriseRemoved == true) || (padapter->bDriverStopped == true)) + { + /* DBG_8723A("%s bSurpriseRemoved:%d, bDriverStopped:%d\n", __func__, padapter->bSurpriseRemoved, padapter->bDriverStopped); */ + return; + } + schedule_work(&pLed->BlinkWorkItem); +} + +/* */ +/* Description: */ +/* Callback function of LED BlinkWorkItem. */ +/* We dispatch acture LED blink action according to LedStrategy. */ +/* */ +void BlinkWorkItemCallback23a(struct work_struct *work) +{ + struct led_8723a *pLed = container_of(work, struct led_8723a, BlinkWorkItem); + BlinkHandler23a(pLed); +} + +/* */ +/* Description: */ +/* Reset status of led_8723a object. */ +/* */ +void ResetLedStatus23a(struct led_8723a * 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_8723a object. */ +/* */ +void +InitLed871x23a(struct rtw_adapter *padapter, struct led_8723a *pLed, enum led_pin_8723a LedPin) +{ + pLed->padapter = padapter; + pLed->LedPin = LedPin; + + ResetLedStatus23a(pLed); + + setup_timer(&pLed->BlinkTimer, BlinkTimerCallback, (unsigned long)pLed); + + INIT_WORK(&pLed->BlinkWorkItem, BlinkWorkItemCallback23a); +} + +/* */ +/* Description: */ +/* DeInitialize an led_8723a object. */ +/* */ +void +DeInitLed871x23a(struct led_8723a *pLed) +{ + cancel_work_sync(&pLed->BlinkWorkItem); + del_timer_sync(&pLed->BlinkTimer); + ResetLedStatus23a(pLed); +} + +/* Description: */ +/* Implementation of LED blinking behavior. */ +/* It toggle off LED and schedule corresponding timer if necessary. */ + +static void SwLedBlink(struct led_8723a *pLed) +{ + struct rtw_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) { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(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 ((check_fwstate(pmlmepriv, _FW_LINKED)) && !pLed->bLedOn) + SwLedOn23a(padapter, pLed); + else if ((check_fwstate(pmlmepriv, _FW_LINKED)) && pLed->bLedOn) + SwLedOff23a(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: + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL)); + break; + case LED_BLINK_SLOWLY: + case LED_BLINK_StartToBlink: + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL)); + break; + case LED_BLINK_WPS: + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_LONG_INTERVAL)); + break; + default: + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL)); + break; + } + } +} + +static void SwLedBlink1(struct led_8723a *pLed) +{ + struct rtw_adapter *padapter = pLed->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + unsigned long delay = 0; + u8 bStopBlinking = false; + + /* Change LED according to BlinkingLedState specified. */ + if (pLed->BlinkingLedState == RTW_LED_ON) { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, + ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, + ("Blinktimes (%d): turn off\n", pLed->BlinkTimes)); + } + + if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) { + SwLedOff23a(padapter, pLed); + ResetLedStatus23a(pLed); + return; + } + switch (pLed->CurrLedState) { + case LED_BLINK_SLOWLY: + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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; + delay = 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) == true) { + pLed->bLedLinkBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_NORMAL; + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) == false) { + pLed->bLedNoLinkBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_SLOWLY; + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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; + delay = 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) == true) { + pLed->bLedLinkBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_NORMAL; + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) == false) { + pLed->bLedNoLinkBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_SLOWLY; + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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; + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + break; + case LED_BLINK_WPS: + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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; + delay = 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; + delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA; + } + break; + default: + break; + } + if (delay) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); +} + +static void SwLedBlink2(struct led_8723a *pLed) +{ + struct rtw_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) { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, + ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(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) { + SwLedOff23a(padapter, pLed); + } else if (check_fwstate(pmlmepriv, _FW_LINKED)) { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + SwLedOn23a(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; + SwLedOff23a(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) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(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) { + SwLedOff23a(padapter, pLed); + } else if (check_fwstate(pmlmepriv, _FW_LINKED)) { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + SwLedOn23a(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; + SwLedOff23a(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) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA)); + } + } + break; + default: + break; + } +} + +static void SwLedBlink3(struct led_8723a *pLed) +{ + struct rtw_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) + { + SwLedOn23a(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) + SwLedOff23a(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) + { + SwLedOff23a(padapter, pLed); + } + else if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + if (!pLed->bLedOn) + SwLedOn23a(padapter, pLed); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState)); + } + else if (check_fwstate(pmlmepriv, _FW_LINKED) == false) + { + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + if (pLed->bLedOn) + SwLedOff23a(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) + { + SwLedOff23a(padapter, pLed); + } + else + { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(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) + { + SwLedOff23a(padapter, pLed); + } + else if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + + if (!pLed->bLedOn) + SwLedOn23a(padapter, pLed); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState)); + } + else if (check_fwstate(pmlmepriv, _FW_LINKED) == false) + { + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + + if (pLed->bLedOn) + SwLedOff23a(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) + { + SwLedOff23a(padapter, pLed); + } + else + { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA)); + } + } + break; + + case LED_BLINK_WPS: + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA)); + break; + + case LED_BLINK_WPS_STOP: /* WPS success */ + if (pLed->BlinkingLedState == RTW_LED_ON) + { + pLed->BlinkingLedState = RTW_LED_OFF; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA)); + bStopBlinking = false; + } else { + bStopBlinking = true; + } + + if (bStopBlinking) + { + if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) + { + SwLedOff23a(padapter, pLed); + } + else + { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + SwLedOn23a(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_8723a *pLed) +{ + struct rtw_adapter *padapter = pLed->padapter; + struct led_priv *ledpriv = &padapter->ledpriv; + struct led_8723a *pLed1 = &ledpriv->SwLed1; + u8 bStopBlinking = false; + unsigned long delay = 0; + + /* Change LED according to BlinkingLedState specified. */ + if (pLed->BlinkingLedState == RTW_LED_ON) + { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(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; + SwLedOff23a(padapter, pLed1); + } + + switch (pLed->CurrLedState) + { + case LED_BLINK_SLOWLY: + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA; + break; + + case LED_BLINK_StartToBlink: + if (pLed->bLedOn) { + pLed->BlinkingLedState = RTW_LED_OFF; + delay = LED_BLINK_SLOWLY_INTERVAL; + } else { + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) { + SwLedOff23a(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; + delay = 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) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) { + SwLedOff23a(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; + delay = 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) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + } + break; + + case LED_BLINK_WPS: + if (pLed->bLedOn) { + pLed->BlinkingLedState = RTW_LED_OFF; + delay = LED_BLINK_SLOWLY_INTERVAL; + } else { + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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; + + delay = 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; + delay = LED_BLINK_LINK_INTERVAL_ALPHA; + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + + delay = LED_BLINK_NORMAL_INTERVAL; + } + break; + + default: + break; + } + if (delay) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState)); +} + +static void SwLedBlink5(struct led_8723a *pLed) +{ + struct rtw_adapter *padapter = pLed->padapter; + u8 bStopBlinking = false; + unsigned long delay = 0; + + /* Change LED according to BlinkingLedState specified. */ + if (pLed->BlinkingLedState == RTW_LED_ON) { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(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) + SwLedOff23a(padapter, pLed); + } else { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + if (!pLed->bLedOn) + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + + pLed->bLedScanBlinkInProgress = false; + } else { + if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) + SwLedOff23a(padapter, pLed); + } else { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + if (!pLed->bLedOn) + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + + pLed->bLedBlinkInProgress = false; + } else { + if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) { + SwLedOff23a(padapter, pLed); + } else { + if (pLed->bLedOn) + pLed->BlinkingLedState = RTW_LED_OFF; + else + pLed->BlinkingLedState = RTW_LED_ON; + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + } + break; + + default: + break; + } + + if (delay) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState)); +} + +static void SwLedBlink6(struct led_8723a *pLed) +{ + struct rtw_adapter *padapter = pLed->padapter; + + /* Change LED according to BlinkingLedState specified. */ + if (pLed->BlinkingLedState == RTW_LED_ON) { + SwLedOn23a(padapter, pLed); + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes)); + } else { + SwLedOff23a(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 rtw_adapter *padapter, enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct led_8723a *pLed = &ledpriv->SwLed0; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + long delay = -1; + + switch (LedAction) + { + case LED_CTL_POWER_ON: + case LED_CTL_START_TO_LINK: + case LED_CTL_NO_LINK: + if (pLed->bLedNoLinkBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_NO_LINK_INTERVAL_ALPHA; + } + break; + + case LED_CTL_LINK: + if (pLed->bLedLinkBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_LINK_INTERVAL_ALPHA; + } + break; + + case LED_CTL_SITE_SURVEY: + if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true)) + ; + else if (pLed->bLedScanBlinkInProgress == false) { + if (IS_LED_WPS_BLINKING(pLed)) + return; + + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_SCAN_INTERVAL_ALPHA; + } + break; + + case LED_CTL_TX: + case LED_CTL_RX: + if (pLed->bLedBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + break; + + case LED_CTL_START_WPS: /* wait until xinpin finish */ + case LED_CTL_START_WPS_BOTTON: + if (pLed->bLedWPSBlinkInProgress == false) { + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_SCAN_INTERVAL_ALPHA; + } + break; + + case LED_CTL_STOP_WPS: + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + } else { + pLed->bLedWPSBlinkInProgress = true; + } + + pLed->CurrLedState = LED_BLINK_WPS_STOP; + if (pLed->bLedOn) { + pLed->BlinkingLedState = RTW_LED_OFF; + delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA; + } else { + pLed->BlinkingLedState = RTW_LED_ON; + delay = 0; + } + break; + + case LED_CTL_STOP_WPS_FAIL: + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&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; + delay = 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) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + + SwLedOff23a(padapter, pLed); + break; + + default: + break; + + } + + if (delay != -1) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState)); +} + + /* Arcadyan/Sitecom , added by chiyoko, 20090216 */ +static void +SwLedControlMode2(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct led_8723a *pLed = &ledpriv->SwLed0; + long delay = -1; + + switch (LedAction) { + case LED_CTL_SITE_SURVEY: + if (pmlmepriv->LinkDetectInfo.bBusyTraffic) + ; + else if (pLed->bLedScanBlinkInProgress == false) { + if (IS_LED_WPS_BLINKING(pLed)) + return; + + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_SCAN_INTERVAL_ALPHA; + } + break; + case LED_CTL_TX: + case LED_CTL_RX: + if ((pLed->bLedBlinkInProgress == false) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true)) { + 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; + delay = LED_BLINK_FASTER_INTERVAL_ALPHA; + } + break; + case LED_CTL_LINK: + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + + delay = 0; + break; + case LED_CTL_START_WPS: /* wait until xinpin finish */ + case LED_CTL_START_WPS_BOTTON: + if (pLed->bLedWPSBlinkInProgress == false) { + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + pLed->bLedWPSBlinkInProgress = true; + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + delay = 0; + } + break; + case LED_CTL_STOP_WPS: + pLed->bLedWPSBlinkInProgress = false; + if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) { + SwLedOff23a(padapter, pLed); + } else { + pLed->CurrLedState = RTW_LED_ON; + pLed->BlinkingLedState = RTW_LED_ON; + delay = 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) { + SwLedOff23a(padapter, pLed); + } else { + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + delay = 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; + delay = 0; + } + break; + case LED_CTL_POWER_OFF: + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } + + delay = 0; + break; + default: + break; + + } + + if (delay != -1) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState)); +} + + /* COREGA, added by chiyoko, 20090316 */ +static void +SwLedControlMode3(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct led_8723a *pLed = &ledpriv->SwLed0; + long delay = -1; + + switch (LedAction) + { + case LED_CTL_SITE_SURVEY: + if (pmlmepriv->LinkDetectInfo.bBusyTraffic) + ; + else if (pLed->bLedScanBlinkInProgress == false) { + if (IS_LED_WPS_BLINKING(pLed)) + return; + + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_SCAN_INTERVAL_ALPHA; + } + break; + + case LED_CTL_TX: + case LED_CTL_RX: + if ((pLed->bLedBlinkInProgress == false) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true)) { + 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; + delay = 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) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + + delay = 0; + break; + + case LED_CTL_START_WPS: /* wait until xinpin finish */ + case LED_CTL_START_WPS_BOTTON: + if (pLed->bLedWPSBlinkInProgress == false) { + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress == true) { + del_timer_sync(&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; + delay = LED_BLINK_SCAN_INTERVAL_ALPHA; + } + break; + + case LED_CTL_STOP_WPS: + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } else { + pLed->bLedWPSBlinkInProgress = true; + } + + pLed->CurrLedState = LED_BLINK_WPS_STOP; + if (pLed->bLedOn) { + pLed->BlinkingLedState = RTW_LED_OFF; + delay = LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA; + } else { + pLed->BlinkingLedState = RTW_LED_ON; + delay = 0; + } + + break; + + case LED_CTL_STOP_WPS_FAIL: + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } + + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + delay = 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; + delay = 0; + } + break; + + case LED_CTL_POWER_OFF: + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } + + delay = 0; + break; + + default: + break; + + } + + if (delay != -1) + mod_timer(&pLed->BlinkTimer, jiffies + msecs_to_jiffies(delay)); + + RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState)); +} + + /* Edimax-Belkin, added by chiyoko, 20090413 */ +static void +SwLedControlMode4(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct led_8723a *pLed = &ledpriv->SwLed0; + struct led_8723a *pLed1 = &ledpriv->SwLed1; + + switch (LedAction) + { + case LED_CTL_START_TO_LINK: + if (pLed1->bLedWPSBlinkInProgress) { + pLed1->bLedWPSBlinkInProgress = false; + del_timer_sync(&pLed1->BlinkTimer); + + pLed1->BlinkingLedState = RTW_LED_OFF; + pLed1->CurrLedState = RTW_LED_OFF; + + if (pLed1->bLedOn) + mod_timer(&pLed->BlinkTimer, jiffies); + } + + if (pLed->bLedStartToLinkBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + + pLed->bLedStartToLinkBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_StartToBlink; + if (pLed->bLedOn) { + pLed->BlinkingLedState = RTW_LED_OFF; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL)); + } else { + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, + jiffies + msecs_to_jiffies(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; + del_timer_sync(&pLed1->BlinkTimer); + + pLed1->BlinkingLedState = RTW_LED_OFF; + pLed1->CurrLedState = RTW_LED_OFF; + + if (pLed1->bLedOn) + mod_timer(&pLed->BlinkTimer, jiffies); + } + } + + if (pLed->bLedNoLinkBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA)); + } + break; + + case LED_CTL_SITE_SURVEY: + if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true)) + ; + else if (pLed->bLedScanBlinkInProgress == false) { + if (IS_LED_WPS_BLINKING(pLed)) + return; + + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA)); + } + break; + + case LED_CTL_TX: + case LED_CTL_RX: + if (pLed->bLedBlinkInProgress == false) { + if (pLed->CurrLedState == LED_BLINK_SCAN || + IS_LED_WPS_BLINKING(pLed)) { + return; + } + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(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; + del_timer_sync(&pLed1->BlinkTimer); + + pLed1->BlinkingLedState = RTW_LED_OFF; + pLed1->CurrLedState = RTW_LED_OFF; + + if (pLed1->bLedOn) + mod_timer(&pLed->BlinkTimer, jiffies); + } + + if (pLed->bLedWPSBlinkInProgress == false) { + if (pLed->bLedNoLinkBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress == true) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + pLed->bLedWPSBlinkInProgress = true; + pLed->CurrLedState = LED_BLINK_WPS; + if (pLed->bLedOn) + { + pLed->BlinkingLedState = RTW_LED_OFF; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SLOWLY_INTERVAL)); + } else { + pLed->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL)); + } + } + break; + + case LED_CTL_STOP_WPS: /* WPS connect success */ + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA)); + break; + + case LED_CTL_STOP_WPS_FAIL: /* WPS authentication fail */ + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA)); + + /* LED1 settings */ + if (pLed1->bLedWPSBlinkInProgress) + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL)); + + break; + + case LED_CTL_STOP_WPS_FAIL_OVERLAP: /* WPS session overlap */ + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA)); + + /* LED1 settings */ + if (pLed1->bLedWPSBlinkInProgress) + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_NORMAL_INTERVAL)); + + break; + + case LED_CTL_POWER_OFF: + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + + if (pLed->bLedNoLinkBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedNoLinkBlinkInProgress = false; + } + if (pLed->bLedLinkBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedLinkBlinkInProgress = false; + } + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + if (pLed->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedWPSBlinkInProgress = false; + } + if (pLed->bLedScanBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedScanBlinkInProgress = false; + } + if (pLed->bLedStartToLinkBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedStartToLinkBlinkInProgress = false; + } + + if (pLed1->bLedWPSBlinkInProgress) { + del_timer_sync(&pLed1->BlinkTimer); + pLed1->bLedWPSBlinkInProgress = false; + } + + pLed1->BlinkingLedState = LED_UNKNOWN; + SwLedOff23a(padapter, pLed); + SwLedOff23a(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 rtw_adapter *padapter, enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct led_8723a *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; + + mod_timer(&pLed->BlinkTimer, jiffies); + break; + + case LED_CTL_SITE_SURVEY: + if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED) == true)) + ; + else if (pLed->bLedScanBlinkInProgress == false) + { + if (pLed->bLedBlinkInProgress == true) + { + del_timer_sync(&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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA)); + } + break; + + case LED_CTL_TX: + case LED_CTL_RX: + if (pLed->bLedBlinkInProgress == false) { + 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; + mod_timer(&pLed->BlinkTimer, jiffies + + msecs_to_jiffies(LED_BLINK_FASTER_INTERVAL_ALPHA)); + } + break; + + case LED_CTL_POWER_OFF: + pLed->CurrLedState = RTW_LED_OFF; + pLed->BlinkingLedState = RTW_LED_OFF; + + if (pLed->bLedBlinkInProgress) { + del_timer_sync(&pLed->BlinkTimer); + pLed->bLedBlinkInProgress = false; + } + + SwLedOff23a(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 rtw_adapter *padapter, + enum led_ctl_mode LedAction) +{ + struct led_priv *ledpriv = &padapter->ledpriv; + struct led_8723a *pLed0 = &ledpriv->SwLed0; + + switch (LedAction) { + case LED_CTL_POWER_ON: + case LED_CTL_LINK: + case LED_CTL_NO_LINK: + del_timer_sync(&pLed0->BlinkTimer); + pLed0->CurrLedState = RTW_LED_ON; + pLed0->BlinkingLedState = RTW_LED_ON; + mod_timer(&pLed0->BlinkTimer, jiffies); + break; + case LED_CTL_POWER_OFF: + SwLedOff23a(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 BlinkHandler23a(struct led_8723a *pLed) +{ + struct rtw_adapter *padapter = pLed->padapter; + struct led_priv *ledpriv = &padapter->ledpriv; + + /* DBG_8723A("%s (%s:%d)\n", __func__, current->comm, current->pid); */ + + 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 +LedControl871x23a(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) { + struct led_priv *ledpriv = &padapter->ledpriv; + + if ((padapter->bSurpriseRemoved == true) || + (padapter->bDriverStopped == true) || + (padapter->hw_init_completed == false)) { + return; + } + + if (ledpriv->bRegUseLed == false) + return; + + /* if (!priv->up) */ + /* return; */ + + /* if (priv->bInHctTest) */ + /* 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/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c new file mode 100644 index 000000000000..71749a37a78e --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_mlme.c @@ -0,0 +1,2499 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_MLME_C_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern u8 rtw_do_join23a(struct rtw_adapter * padapter); + +static void rtw_init_mlme_timer(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + setup_timer(&pmlmepriv->assoc_timer, rtw23a_join_to_handler, + (unsigned long)padapter); + + setup_timer(&pmlmepriv->scan_to_timer, rtw_scan_timeout_handler23a, + (unsigned long)padapter); + + setup_timer(&pmlmepriv->dynamic_chk_timer, + rtw_dynamic_check_timer_handler, (unsigned long)padapter); + + setup_timer(&pmlmepriv->set_scan_deny_timer, + rtw_set_scan_deny_timer_hdl, (unsigned long)padapter); +} + +int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + int res = _SUCCESS; + + pmlmepriv->nic_hdl = padapter; + + 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_queue23a(&pmlmepriv->scanned_queue); + + memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct cfg80211_ssid)); + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + + rtw_clear_scan_deny(padapter); + + rtw_init_mlme_timer(padapter); + return res; +} + +static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen) +{ + if(*ppie) + { + kfree(*ppie); + *plen = 0; + *ppie=NULL; + } +} + +void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv) +{ +#ifdef CONFIG_8723AU_AP_MODE + kfree(pmlmepriv->assoc_req); + kfree(pmlmepriv->assoc_rsp); + 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); +#endif + +#if defined(CONFIG_8723AU_P2P) + rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len); +#endif +} + +void _rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv) +{ + + rtw23a_free_mlme_priv_ie_data(pmlmepriv); + +} + +struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv) +{ + struct wlan_network *pnetwork; + + pnetwork = kzalloc(sizeof(struct wlan_network), GFP_ATOMIC); + if (pnetwork) { + INIT_LIST_HEAD(&pnetwork->list); + pnetwork->network_type = 0; + pnetwork->fixed = false; + pnetwork->last_scanned = jiffies; + pnetwork->aid = 0; + pnetwork->join_res = 0; + } + + return pnetwork; +} + +void _rtw_free_network23a(struct mlme_priv *pmlmepriv, + struct wlan_network *pnetwork, u8 isfreeall) +{ + u32 lifetime = SCANQUEUE_LIFETIME; + + if (!pnetwork) + return; + + if (pnetwork->fixed == true) + return; + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) + lifetime = 1; + + list_del_init(&pnetwork->list); + + kfree(pnetwork); +} + +void _rtw_free_network23a_nolock23a(struct mlme_priv *pmlmepriv, + struct wlan_network *pnetwork) +{ + + if (pnetwork == NULL) + return; + + if (pnetwork->fixed == true) + return; + + list_del_init(&pnetwork->list); + + kfree(pnetwork); +} + +/* + return the wlan_network with the matching addr + + Shall be calle under atomic context... to avoid possible racing condition... +*/ +struct wlan_network * +_rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr) +{ + struct list_head *phead, *plist; + struct wlan_network *pnetwork = NULL; + + if (is_zero_ether_addr(addr)) { + pnetwork = NULL; + goto exit; + } + + /* spin_lock_bh(&scanned_queue->lock); */ + + phead = get_list_head(scanned_queue); + plist = phead->next; + + while (plist != phead) { + pnetwork = container_of(plist, struct wlan_network, list); + + if (ether_addr_equal(addr, pnetwork->network.MacAddress)) + break; + + plist = plist->next; + } + + if(plist == phead) + pnetwork = NULL; + + /* spin_unlock_bh(&scanned_queue->lock); */ + +exit: + + return pnetwork; +} + +void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall) +{ + struct list_head *phead, *plist, *ptmp; + struct wlan_network *pnetwork; + struct mlme_priv* pmlmepriv = &padapter->mlmepriv; + struct rtw_queue *scanned_queue = &pmlmepriv->scanned_queue; + + spin_lock_bh(&scanned_queue->lock); + + phead = get_list_head(scanned_queue); + + list_for_each_safe(plist, ptmp, phead) { + pnetwork = container_of(plist, struct wlan_network, list); + + _rtw_free_network23a(pmlmepriv,pnetwork, isfreeall); + } + + spin_unlock_bh(&scanned_queue->lock); + +} + +int rtw_if_up23a(struct rtw_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_up23a:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved)); + res=false; + } + else + res= true; + + return res; +} + +void rtw_generate_random_ibss23a(u8* pibss) +{ + unsigned long 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_capability23a_from_ie(u8 *ie) +{ + return ie + 8 + 2; +} + +u16 rtw_get_capability23a(struct wlan_bssid_ex *bss) +{ + u16 val; + + memcpy((u8 *)&val, rtw_get_capability23a_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); +} + +u8 *rtw_get_timestampe_from_ie23a(u8 *ie) +{ + return ie + 0; +} + +u8 *rtw_get_beacon_interval23a_from_ie(u8 *ie) +{ + return ie + 8; +} + +int rtw_init_mlme_priv23a (struct rtw_adapter *padapter)/* struct mlme_priv *pmlmepriv) */ +{ + int res; + + res = _rtw_init_mlme_priv23a(padapter);/* (pmlmepriv); */ + + return res; +} + +void rtw_free_mlme_priv23a (struct mlme_priv *pmlmepriv) +{ + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv23a\n")); + _rtw_free_mlme_priv23a(pmlmepriv); + +} + +void rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 is_freeall); +void rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 is_freeall)/* struct wlan_network *pnetwork, _queue *free_queue) */ +{ + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("rtw_free_network ==> ssid = %s\n\n" , + pnetwork->network.Ssid.ssid)); + _rtw_free_network23a(pmlmepriv, pnetwork, is_freeall); + +} + +void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork); +void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork) +{ + + /* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_network ==> ssid = %s\n\n" , pnetwork->network.Ssid.ssid)); */ + _rtw_free_network23a_nolock23a(pmlmepriv, pnetwork); + +} + +void rtw_free_network_queue23a(struct rtw_adapter* dev, u8 isfreeall) +{ + + _rtw_free_network23a_queue23a(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_network23a(struct rtw_queue *scanned_queue, u8 *addr) +{ + struct wlan_network *pnetwork; + + pnetwork = _rtw_find_network23a(scanned_queue, addr); + + return pnetwork; +} + +int rtw_is_same_ibss23a(struct rtw_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; +} + +inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b); +inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b) +{ + /* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("(%s,%d)(%s,%d)\n", */ + /* a->Ssid.Ssid, a->Ssid.SsidLength, b->Ssid.Ssid, b->Ssid.SsidLength)); */ + return (a->Ssid.ssid_len == b->Ssid.ssid_len) && + !memcmp(a->Ssid.ssid, b->Ssid.ssid, a->Ssid.ssid_len); +} + +int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst) +{ + u16 s_cap, d_cap; + + memcpy((u8 *)&s_cap, rtw_get_capability23a_from_ie(src->IEs), 2); + memcpy((u8 *)&d_cap, rtw_get_capability23a_from_ie(dst->IEs), 2); + + s_cap = le16_to_cpu(s_cap); + d_cap = le16_to_cpu(d_cap); + + return ((src->Ssid.ssid_len == dst->Ssid.ssid_len) && + /* (src->Configuration.DSConfig == dst->Configuration.DSConfig) && */ + ether_addr_equal(src->MacAddress, dst->MacAddress) && + ((!memcmp(src->Ssid.ssid, dst->Ssid.ssid, src->Ssid.ssid_len))) && + ((s_cap & WLAN_CAPABILITY_IBSS) == + (d_cap & WLAN_CAPABILITY_IBSS)) && + ((s_cap & WLAN_CAPABILITY_ESS) == + (d_cap & WLAN_CAPABILITY_ESS))); +} + +struct wlan_network *rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue) +{ + struct list_head *plist, *phead; + + struct wlan_network *pwlan; + struct wlan_network *oldest = NULL; + + phead = get_list_head(scanned_queue); + + list_for_each(plist, phead) { + pwlan = container_of(plist, struct wlan_network, list); + + if (pwlan->fixed != true) { + if (!oldest || time_after(oldest->last_scanned, + pwlan->last_scanned)) + oldest = pwlan; + } + } + + return oldest; +} + +void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src, + struct rtw_adapter * padapter, bool update_ie) +{ + u8 ss_ori = dst->PhyInfo.SignalStrength; + u8 sq_ori = dst->PhyInfo.SignalQuality; + long rssi_ori = dst->Rssi; + + u8 ss_smp = src->PhyInfo.SignalStrength; + u8 sq_smp = src->PhyInfo.SignalQuality; + long rssi_smp = src->Rssi; + + u8 ss_final; + u8 sq_final; + long rssi_final; + + DBG_8723A("%s %s(%pM, ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n", + __func__, src->Ssid.ssid, src->MacAddress, + src->Configuration.DSConfig, ss_ori, sq_ori, rssi_ori, + ss_smp, sq_smp, rssi_smp + ); + + /* The rule below is 1/5 for sample value, 4/5 for history value */ + if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network23a(&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 = (src->Rssi+dst->Rssi*4)/5; + else + rssi_final = rssi_ori; + } + else { + if (sq_smp != 101) { /* from the right channel */ + ss_final = ((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5; + sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5; + rssi_final = (src->Rssi+dst->Rssi*4)/5; + } else { + /* bss info not receving 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) + 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; + + DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n", + __func__, dst->Ssid.ssid, dst->MacAddress, + dst->PhyInfo.SignalStrength, + dst->PhyInfo.SignalQuality, dst->Rssi); + +} + +static void update_current_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + if ((check_fwstate(pmlmepriv, _FW_LINKED)== true) && (is_same_network23a(&pmlmepriv->cur_network.network, pnetwork))) + { + /* RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"Same Network\n"); */ + + /* if(pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */ + { + update_network23a(&pmlmepriv->cur_network.network, pnetwork,adapter, true); + rtw_update_protection23a(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof (struct ndis_802_11_fixed_ies), + pmlmepriv->cur_network.network.IELength); + } + } + +} + +/* + +Caller must hold pmlmepriv->lock first. + +*/ +void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssid_ex *target) +{ + struct list_head *plist, *phead; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *pnetwork = NULL; + struct wlan_network *oldest = NULL; + struct rtw_queue *queue = &pmlmepriv->scanned_queue; + u32 bssid_ex_sz; + int found = 0; + + spin_lock_bh(&queue->lock); + phead = get_list_head(queue); + + list_for_each(plist, phead) { + pnetwork = container_of(plist, struct wlan_network, list); + + if (is_same_network23a(&pnetwork->network, target)) { + found = 1; + break; + } + if (!oldest || time_after(oldest->last_scanned, + pnetwork->last_scanned)) + oldest = pnetwork; + } + + /* If we didn't find a match, then get a new network slot to initialize + * with this beacon's information */ + if (!found) { + pnetwork = rtw_alloc_network(pmlmepriv); + if (!pnetwork) { + if (!oldest) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("\n\n\nsomething wrong here\n\n\n")); + goto exit; + } + pnetwork = oldest; + } else + list_add_tail(&pnetwork->list, &queue->queue); + + bssid_ex_sz = get_wlan_bssid_ex_sz(target); + target->Length = bssid_ex_sz; + memcpy(&pnetwork->network, target, bssid_ex_sz); + + /* variable initialize */ + pnetwork->fixed = false; + pnetwork->last_scanned = jiffies; + + pnetwork->network_type = 0; + pnetwork->aid = 0; + pnetwork->join_res = 0; + + /* bss info not receving from the right channel */ + if (pnetwork->network.PhyInfo.SignalQuality == 101) + pnetwork->network.PhyInfo.SignalQuality = 0; + } 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 == 1, means that scanned network is + * a bcn frame. */ + if ((pnetwork->network.IELength>target->IELength) && + (target->reserved == 1)) + update_ie = false; + + update_network23a(&pnetwork->network, target,adapter, update_ie); + } + +exit: + spin_unlock_bh(&queue->lock); + +} + +void rtw_add_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork) +{ + update_current_network(adapter, pnetwork); + rtw_update_scanned_network23a(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 */ +int rtw_is_desired_network(struct rtw_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_ie23a(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.... */ + { + if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0)) + bselected = false; + } + + if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) { + DBG_8723A("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_callback23a(struct rtw_adapter *adapter , u8 *pbuf) +{ + + RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("receive atimdone_evet\n")); + + return; +} + +void rtw_survey_event_cb23a(struct rtw_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_cb23a, 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_cb23a: 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) + { + /* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */ + if (ether_addr_equal(pmlmepriv->cur_network.network.MacAddress, + pnetwork->MacAddress)) { + 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_network23a(&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.ssid_len = 0; + + rtw_add_network(adapter, pnetwork); + } + +exit: + + spin_unlock_bh(&pmlmepriv->lock); + + return; +} + +void rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + + 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_callback23a: fw_state:%x\n\n", get_fwstate(pmlmepriv))); + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) { + del_timer_sync(&pmlmepriv->scan_to_timer); + + _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))); + } + + rtw_set_signal_stat_timer(&adapter->recvpriv); + + if (pmlmepriv->to_join == true) { + 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_queue23a(pmlmepriv) == _SUCCESS) { + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(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 cfg80211_ssid)); + memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct cfg80211_ssid)); + + rtw_update_registrypriv_dev_network23a(adapter); + rtw_generate_random_ibss23a(pibss); + + pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE; + + if (rtw_createbss_cmd23a(adapter)!= _SUCCESS) + { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd23a status FAIL\n")); + } + + pmlmepriv->to_join = false; + } + } + } else { + int ret; + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + pmlmepriv->to_join = false; + ret = rtw_select_and_join_from_scanned_queue23a(pmlmepriv); + if (ret == _SUCCESS) { + unsigned long e; + e = msecs_to_jiffies(MAX_JOIN_TIMEOUT); + mod_timer(&pmlmepriv->assoc_timer, jiffies + e); + } else if (ret == 2)/* there is no need to wait for join */ + { + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + rtw_indicate_connect23a(adapter); + } else { + DBG_8723A("try_to_join, but select scanning queue fail, to_roaming:%d\n", rtw_to_roaming(adapter)); + if (rtw_to_roaming(adapter) != 0) { + if (--pmlmepriv->to_roaming == 0 + || _SUCCESS != rtw_sitesurvey_cmd23a(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0) + ) { + rtw_set_roaming(adapter, 0); + rtw_free_assoc_resources23a(adapter, 1); + rtw_indicate_disconnect23a(adapter); + } else { + pmlmepriv->to_join = true; + } + } + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + } + } + } + + spin_unlock_bh(&pmlmepriv->lock); + +#ifdef CONFIG_8723AU_P2P + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + p2p_ps_wk_cmd23a(adapter, P2P_PS_SCAN_DONE, 0); +#endif /* CONFIG_8723AU_P2P */ + + rtw_os_xmit_schedule23a(adapter); + + if(pmlmeext->sitesurvey_res.bss_cnt == 0) + rtw_hal_sreset_reset23a(adapter); + + rtw_cfg80211_surveydone_event_callback(adapter); + +} + +void rtw_dummy_event_callback23a(struct rtw_adapter *adapter , u8 *pbuf) +{ +} + +void rtw23a_fwdbg_event_callback(struct rtw_adapter *adapter , u8 *pbuf) +{ +} + +static void free_scanqueue(struct mlme_priv *pmlmepriv) +{ + struct wlan_network *pnetwork; + struct rtw_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); + + phead = get_list_head(scan_queue); + + list_for_each_safe(plist, ptemp, phead) { + list_del_init(plist); + pnetwork = container_of(plist, struct wlan_network, list); + kfree(pnetwork); + } + + spin_unlock_bh(&scan_queue->lock); + +} + +/* +*rtw_free_assoc_resources23a: the caller has to lock pmlmepriv->lock +*/ +void rtw_free_assoc_resources23a(struct rtw_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_resources23a\n")); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n", + MAC_ARG(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_stainfo23a(&adapter->stapriv, tgt_network->network.MacAddress); + + { + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(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_stainfo23a(adapter); + + psta = rtw_get_bcmc_stainfo23a(adapter); + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(adapter, psta); + spin_unlock_bh(&pstapriv->sta_hash_lock); + + rtw_init_bcmc_stainfo23a(adapter); + } + + if(lock_scanned_queue) + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + + pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress); + if(pwlan) + pwlan->fixed = false; + else + RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_assoc_resources23a : 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_connect23a: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_connect23a(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect23a\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_connect23a(padapter); + } + + rtw_set_roaming(padapter, 0); + + rtw_set_scan_deny(padapter, 3000); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect23a: fw_state=0x%08x\n", get_fwstate(pmlmepriv))); + +} + +/* +*rtw_indicate_disconnect23a: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_disconnect23a(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect23a\n")); + + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS); + + /* DBG_8723A("clear wps when %s\n", __func__); */ + + if (rtw_to_roaming(padapter) > 0) + _clr_fwstate_(pmlmepriv, _FW_LINKED); + + if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) || + (rtw_to_roaming(padapter) <= 0)) { + rtw_os_indicate_disconnect23a(padapter); + + /* set ips_deny_time to avoid enter IPS before LPS leave */ + padapter->pwrctrlpriv.ips_deny_time = + rtw_get_current_time() + rtw_ms_to_systime23a(3000); + + _clr_fwstate_(pmlmepriv, _FW_LINKED); + + rtw_led_control(padapter, LED_CTL_NO_LINK); + + rtw_clear_scan_deny(padapter); + + } + +#ifdef CONFIG_8723AU_P2P + p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1); +#endif /* CONFIG_8723AU_P2P */ + + rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_DISCONNECT, 1); + +} + +inline void rtw_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted) +{ + rtw_os_indicate_scan_done23a(padapter, aborted); +} + +void rtw_scan_abort23a(struct rtw_adapter *adapter) +{ + unsigned long 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) && + jiffies_to_msecs(jiffies - start) <= 200) { + + if (adapter->bDriverStopped || adapter->bSurpriseRemoved) + break; + + DBG_8723A(FUNC_NDEV_FMT"fw_state = _FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev)); + msleep(20); + } + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) { + if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved) + DBG_8723A(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev)); + rtw_indicate_scan_done23a(adapter, true); + } + pmlmeext->scan_abort = false; +} + +static struct sta_info *rtw_joinbss_update_stainfo(struct rtw_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_stainfo23a(pstapriv, pnetwork->network.MacAddress); + if (psta == NULL) { + psta = rtw_alloc_stainfo23a(pstapriv, pnetwork->network.MacAddress); + } + + if (psta) /* update ptarget_sta */ + { + DBG_8723A("%s\n", __func__); + + psta->aid = pnetwork->join_res; + psta->mac_id = 0; + + /* sta mode */ + rtw_hal_set_odm_var23a(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, reseting 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_stainfo23a(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_info23a(padapter, psta); + + } + + return psta; +} + +/* pnetwork : returns from rtw23a_joinbss_event_cb */ +/* ptarget_wlan: found from scanned_queue */ +static void rtw_joinbss_update_network23a(struct rtw_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_8723A("%s\n", __func__); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nfw_state:%x, BSSID:"MAC_FMT"\n" + , get_fwstate(pmlmepriv), MAC_ARG(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); + DBG_8723A("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u\n", + __func__, padapter->recvpriv.signal_strength, + padapter->recvpriv.rssi, padapter->recvpriv.signal_qual); + 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_protection23a(padapter, (cur_network->network.IEs) + sizeof (struct ndis_802_11_fixed_ies), + (cur_network->network.IELength)); + + rtw_update_ht_cap23a(padapter, cur_network->network.IEs, cur_network->network.IELength); +} + +/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) */ +/* pnetwork : returns from rtw23a_joinbss_event_cb */ +/* 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_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf) +{ + static u8 retry=0; + 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_registrypriv23a(adapter); + + if (pmlmepriv->assoc_ssid.ssid_len == 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_, + ("@@@@@ rtw23a_joinbss_event_cb for SSid:%s\n", + pmlmepriv->assoc_ssid.ssid)); + } + + if (ether_addr_equal(pnetwork->network.MacAddress, + cur_network->network.MacAddress)) + the_same_macaddr = true; + else + the_same_macaddr = false; + + 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_,("\n rtw23a_joinbss_event_cb !! _enter_critical\n")); + + if(pnetwork->join_res > 0) + { + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + retry = 0; + if (check_fwstate(pmlmepriv,_FW_UNDER_LINKING)) + { + /* s1. find ptarget_wlan */ + if(check_fwstate(pmlmepriv, _FW_LINKED)) + { + if(the_same_macaddr == true) + { + ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + } + else + { + pcur_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + if(pcur_wlan) pcur_wlan->fixed = false; + + pcur_sta = rtw_get_stainfo23a(pstapriv, cur_network->network.MacAddress); + if(pcur_sta) { + spin_lock_bh(&pstapriv->sta_hash_lock); + rtw_free_stainfo23a(adapter, pcur_sta); + spin_unlock_bh(&pstapriv->sta_hash_lock); + } + + ptarget_wlan = rtw_find_network23a(&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_network23a(&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_network23a(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) + { + rtw_indicate_connect23a(adapter); + } else { + /* adhoc mode will rtw_indicate_connect23a when rtw_stassoc_event_callback23a */ + RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv))); + } + + /* s5. Cancle assoc_timer */ + del_timer_sync(&pmlmepriv->assoc_timer); + + RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("Cancle assoc_timer\n")); + } else { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("rtw23a_joinbss_event_cb 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_securitypriv23a(adapter); + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(1)); + + /* rtw_free_assoc_resources23a(adapter, 1); */ + + if((check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) { + 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 */ + mod_timer(&pmlmepriv->assoc_timer, + jiffies + msecs_to_jiffies(1)); + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + } + +ignore_joinbss_callback: + + spin_unlock_bh(&pmlmepriv->lock); +} + +void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, u8 *pbuf) +{ + struct wlan_network *pnetwork = (struct wlan_network *)pbuf; + + mlmeext_joinbss_event_callback23a(adapter, pnetwork->join_res); + + rtw_os_xmit_schedule23a(adapter); + +} + +/* FOR AP , AD-HOC mode */ +void rtw_stassoc_hw_rpt23a(struct rtw_adapter *adapter, struct sta_info *psta) +{ + u16 media_status; + + if (psta == NULL) return; + + media_status = (psta->mac_id<<8)|1; /* MACID|OPMODE:1 connect */ + rtw_hal_set_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status); +} + +void rtw_stassoc_event_callback23a(struct rtw_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_ctrl23a(adapter, pstassoc->macaddr) == false) + return; + +#ifdef CONFIG_8723AU_AP_MODE + if(check_fwstate(pmlmepriv, WIFI_AP_STATE)) + { + psta = rtw_get_stainfo23a(&adapter->stapriv, pstassoc->macaddr); + if (psta) { + /* bss_cap_update_on_sta_join23a(adapter, psta); */ + /* sta_info_update23a(adapter, psta); */ + ap_sta_info_defer_update23a(adapter, psta); + + rtw_stassoc_hw_rpt23a(adapter,psta); + } + return; + } +#endif + /* for AD-HOC mode */ + psta = rtw_get_stainfo23a(&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_callback23a: 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_stainfo23a(&adapter->stapriv, pstassoc->macaddr); + if (psta == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't alloc sta_info when rtw_stassoc_event_callback23a\n")); + return; + } + + /* to do : init sta_info variable */ + psta->qos_option = 0; + psta->mac_id = (uint)pstassoc->cam_id; + /* psta->aid = (uint)pstassoc->cam_id; */ + DBG_8723A("%s\n",__func__); + /* for ad-hoc mode */ + rtw_hal_set_odm_var23a(adapter,HAL_ODM_STA_INFO,psta,true); + + rtw_stassoc_hw_rpt23a(adapter,psta); + + 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)==true ) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==true ) ) + { + if(adapter->stapriv.asoc_sta_count== 2) + { + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + if(ptarget_wlan) ptarget_wlan->fixed = true; + spin_unlock_bh(&pmlmepriv->scanned_queue.lock); + /* a sta + bc/mc_stainfo (not Ibss_stainfo) */ + rtw_indicate_connect23a(adapter); + } + } + + spin_unlock_bh(&pmlmepriv->lock); + + mlmeext_sta_add_event_callback23a(adapter, psta); +} + +void rtw_stadel_event_callback23a(struct rtw_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_stainfo23a(&adapter->stapriv, pstadel->macaddr); + if(psta) + mac_id = psta->mac_id; + else + mac_id = pstadel->mac_id; + + DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id, MAC_ARG(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_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status); + } + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) + { + return; + } + + mlmeext_sta_del_event_callback23a(adapter); + + spin_lock_bh(&pmlmepriv->lock); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + { + 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_queue23a(adapter); + + rtw_free_assoc_resources23a(adapter, 1); + rtw_indicate_disconnect23a(adapter); + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + /* remove the network entry in scanned_queue */ + pwlan = rtw_find_network23a(&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); + + _rtw23a_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_stainfo23a(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_network23a(&pmlmepriv->scanned_queue, pstadel->macaddr); */ + pwlan = rtw_find_network23a(&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 cfg80211_ssid)); + memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, + sizeof(struct cfg80211_ssid)); + + rtw_update_registrypriv_dev_network23a(adapter); + + rtw_generate_random_ibss23a(pibss); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) + { + set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE); + } + + if (rtw_createbss_cmd23a(adapter)!= _SUCCESS) + { + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd23a status FAIL***\n ")); + + } + + } + + } + + spin_unlock_bh(&pmlmepriv->lock); + +} + +void rtw_cpwm_event_callback23a(struct rtw_adapter *padapter, u8 *pbuf) +{ + + RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("+rtw_cpwm_event_callback23a !!!\n")); + +} + +/* +* rtw23a_join_to_handler - Timeout/faliure handler for CMD JoinBss +* @adapter: pointer to _adapter structure +*/ +void rtw23a_join_to_handler (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + int do_join_r; + + DBG_8723A("%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_8723A("%s try another roaming\n", __func__); + if (_SUCCESS!= (do_join_r = rtw_do_join23a(adapter))) { + DBG_8723A("%s roaming do_join return %d\n", __func__ , do_join_r); + continue; + } + break; + } else { + DBG_8723A("%s We've try roaming but fail\n", __func__); + rtw_indicate_disconnect23a(adapter); + break; + } + } + } else { + rtw_indicate_disconnect23a(adapter); + free_scanqueue(pmlmepriv);/* */ + + /* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */ + rtw_cfg80211_indicate_disconnect(adapter); + } + + spin_unlock_bh(&pmlmepriv->lock); + +} + +/* +* rtw_scan_timeout_handler23a - Timeout/Faliure handler for CMD SiteSurvey +* @data: pointer to _adapter structure +*/ +void rtw_scan_timeout_handler23a(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + DBG_8723A(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_done23a(adapter, true); +} + +static void rtw_auto_scan_handler(struct rtw_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_8723A("%s\n", __func__); + rtw_set_802_11_bssid23a_list_scan(padapter, NULL, 0); + pmlmepriv->scan_interval = SCAN_INTERVAL;/* 30*2 sec = 60sec */ + } + } +} + +void rtw_dynamic_check_timer_handler(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct registry_priv *pregistrypriv = &adapter->registrypriv; + + if (adapter->hw_init_completed == false) + goto out; + + if ((adapter->bDriverStopped == true)||(adapter->bSurpriseRemoved == true)) + goto out; + + if (adapter->net_closed == true) + goto out; + + rtw_dynamic_chk_wk_cmd23a(adapter); + + if (pregistrypriv->wifi_spec == 1) + { +#ifdef CONFIG_8723AU_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); + } + } +out: + mod_timer(&adapter->mlmepriv.dynamic_chk_timer, + jiffies + msecs_to_jiffies(2000)); +} + +inline bool rtw_is_scan_deny(struct rtw_adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + return (atomic_read(&mlmepriv->set_scan_deny) != 0) ? true : false; +} + +void rtw_clear_scan_deny(struct rtw_adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + atomic_set(&mlmepriv->set_scan_deny, 0); + if (0) + DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); +} + +void rtw_set_scan_deny_timer_hdl(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + rtw_clear_scan_deny(adapter); +} + +void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + + if (0) + DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); + atomic_set(&mlmepriv->set_scan_deny, 1); + mod_timer(&mlmepriv->set_scan_deny_timer, + jiffies + msecs_to_jiffies(ms)); + +} + +#if defined(IEEE80211_SCAN_RESULT_EXPIRE) +#define RTW_SCAN_RESULT_EXPIRE IEEE80211_SCAN_RESULT_EXPIRE/HZ*1000 -1000 /* 3000 -1000 */ +#else +#define RTW_SCAN_RESULT_EXPIRE 2000 +#endif + +/* +* 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 rtw_adapter *adapter = container_of(pmlmepriv, struct rtw_adapter, mlmepriv); + + /* check bssid, if needed */ + if (pmlmepriv->assoc_by_bssid == true) { + if (!ether_addr_equal(competitor->network.MacAddress, + pmlmepriv->assoc_bssid)) + goto exit; + } + + /* check ssid, if needed */ + if (pmlmepriv->assoc_ssid.ssid && pmlmepriv->assoc_ssid.ssid_len) { + if (competitor->network.Ssid.ssid_len != + pmlmepriv->assoc_ssid.ssid_len || + memcmp(competitor->network.Ssid.ssid, + pmlmepriv->assoc_ssid.ssid, + pmlmepriv->assoc_ssid.ssid_len)) + goto exit; + } + + if (rtw_is_desired_network(adapter, competitor) == false) + goto exit; + + if (rtw_to_roaming(adapter) > 0) { + unsigned int passed; + + passed = jiffies_to_msecs(jiffies - competitor->last_scanned); + if (passed >= RTW_SCAN_RESULT_EXPIRE || + is_same_ess(&competitor->network, + &pmlmepriv->cur_network.network) == false) + goto exit; + } + + if (*candidate == NULL ||(*candidate)->network.Rssinetwork.Rssi) { + *candidate = competitor; + updated = true; + } + + if (updated) { + DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] new candidate: %s("MAC_FMT") rssi:%d\n", + pmlmepriv->assoc_by_bssid, + pmlmepriv->assoc_ssid.ssid, + rtw_to_roaming(adapter), + (*candidate)->network.Ssid.ssid, + MAC_ARG((*candidate)->network.MacAddress), + (int)(*candidate)->network.Rssi); + } + +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_queue23a(struct mlme_priv *pmlmepriv) +{ + int ret; + struct list_head *phead, *plist, *ptmp; + struct rtw_adapter *adapter; + struct rtw_queue *queue = &pmlmepriv->scanned_queue; + struct wlan_network *pnetwork = NULL; + struct wlan_network *candidate = NULL; + + spin_lock_bh(&pmlmepriv->scanned_queue.lock); + phead = get_list_head(queue); + adapter = pmlmepriv->nic_hdl; + + list_for_each_safe(plist, ptmp, phead) { + pnetwork = container_of(plist, struct wlan_network, list); + if (!pnetwork) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("%s return _FAIL:(pnetwork == NULL)\n", + __func__)); + ret = _FAIL; + goto exit; + } + + rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork); + } + + if (!candidate) { + DBG_8723A("%s: return _FAIL(candidate == NULL)\n", __func__); + ret = _FAIL; + goto exit; + } else { + DBG_8723A("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__, + candidate->network.Ssid.ssid, + MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig); + } + + /* check for situation of _FW_LINKED */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + DBG_8723A("%s: _FW_LINKED while ask_for_joinbss!!!\n", + __func__); + + rtw_disassoc_cmd23a(adapter, 0, true); + rtw_indicate_disconnect23a(adapter); + rtw_free_assoc_resources23a(adapter, 0); + } + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + ret = rtw_joinbss_cmd23a(adapter, candidate); + +exit: + spin_unlock_bh(&pmlmepriv->scanned_queue.lock); + + return ret; +} + +int rtw_set_auth23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), GFP_KERNEL); + if (!pcmd) { + res = _FAIL; /* try again */ + goto exit; + } + + psetauthparm = (struct setauth_parm*) + kzalloc(sizeof(struct setauth_parm), GFP_KERNEL); + if (!psetauthparm) { + kfree(pcmd); + res = _FAIL; + goto exit; + } + + 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_cmd23a(pcmdpriv, pcmd); + +exit: + + return res; +} + +int rtw_set_key23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), GFP_KERNEL); + if (!pcmd) { + res = _FAIL; /* try again */ + goto exit; + } + psetkeyparm = (struct setkey_parm *) + kzalloc(sizeof(struct setkey_parm), GFP_KERNEL); + if (!psetkeyparm) { + kfree(pcmd); + res = _FAIL; + goto exit; + } + + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + psetkeyparm->algorithm = (unsigned char) + psecuritypriv->dot118021XGrpPrivacy; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("\n rtw_set_key23a: 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_key23a: psetkeyparm->algorithm = (u8)" + "psecuritypriv->dot11PrivacyAlgrthm =%d\n", + psetkeyparm->algorithm)); + } + psetkeyparm->keyid = (u8)keyid;/* 0~3 */ + psetkeyparm->set_tx = set_tx; + if (is_wep_enc(psetkeyparm->algorithm)) + pmlmepriv->key_mask |= CHKBIT(psetkeyparm->keyid); + + DBG_8723A("==> rtw_set_key23a 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_key23a: 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_key23a: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); + + /* sema_init(&pcmd->cmd_sem, 0); */ + + res = rtw_enqueue_cmd23a(pcmdpriv, pcmd); + +exit: + + return res; +} + +/* adjust IEs for rtw_joinbss_cmd23a in WMM */ +int rtw_restruct_wmm_ie23a(struct rtw_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; + + /* WMM element ID and OUI */ + 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) { + + /* 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 rtw_adapter *Adapter, u8 *bssid) +{ + struct security_priv *psecuritypriv = &Adapter->securitypriv; + int i = 0; + + do { + if (psecuritypriv->PMKIDList[i].bUsed && + ether_addr_equal(psecuritypriv->PMKIDList[i].Bssid, bssid)) { + 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 rtw_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_ie23a(struct rtw_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_ie23a: 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_ie23a(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_network23a(struct rtw_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); + + ether_addr_copy(pdev_network->MacAddress, myhwaddr); + + memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, + sizeof(struct cfg80211_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_network23a(struct rtw_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; + /* struct xmit_priv *pxmitpriv = &adapter->xmitpriv; */ + + pdev_network->Privacy = + (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0); + + 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_ie23a(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_registrypriv23a(struct rtw_adapter* adapter) +{ + +} + +/* the fucntion is at passive_level */ +void rtw_joinbss_reset23a(struct rtw_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_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, + (u8 *)(&threshold)); + } else { + threshold = 1; + rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, + (u8 *)(&threshold)); + } +} + +/* the fucntion is >= passive_level */ +unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie, + u8 *out_ie, uint in_len, uint *pout_len) +{ + u32 ielen, out_len; + int max_rx_ampdu_factor; + unsigned char *p, *pframe; + 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; + + phtpriv->ht_option = false; + + p = rtw_get_ie23a(in_ie + 12, _HT_CAPABILITY_IE_, &ielen, in_len - 12); + + if (p && ielen > 0) { + u32 rx_packet_offset, max_recvbuf_sz; + if (pqospriv->qos_option == 0) { + out_len = *pout_len; + pframe = rtw_set_ie23a(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 = IEEE80211_HT_CAP_SUP_WIDTH_20_40 | + IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 | + IEEE80211_HT_CAP_TX_STBC | IEEE80211_HT_CAP_DSSSCCK40; + + rtw_hal_get_def_var23a(padapter, HAL_DEF_RX_PACKET_OFFSET, + &rx_packet_offset); + rtw_hal_get_def_var23a(padapter, HAL_DEF_MAX_RECVBUF_SZ, + &max_recvbuf_sz); + + rtw_hal_get_def_var23a(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_AMPDU_PARM_DENSITY& (0x07 << 2)); + else + ht_capie.ampdu_params_info |= + (IEEE80211_HT_AMPDU_PARM_DENSITY & 0x00); + + pframe = rtw_set_ie23a(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_ie23a(in_ie + 12, _HT_ADD_INFO_IE_, &ielen, in_len-12); + if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) { + out_len = *pout_len; + pframe = rtw_set_ie23a(out_ie + out_len, _HT_ADD_INFO_IE_, + ielen, p + 2 , pout_len); + } + } + + return phtpriv->ht_option; +} + +/* the fucntion is > passive_level (in critical_section) */ +void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len) +{ + u8 *p, max_ampdu_sz; + int len; + /* struct sta_info *bmc_sta, *psta; */ + struct ieee80211_ht_cap *pht_capie; + struct ieee80211_ht_addt_info *pht_addtinfo; + /* struct recv_reorder_ctrl *preorder_ctrl; */ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + /* struct recv_priv *precvpriv = &padapter->recvpriv; */ + struct registry_priv *pregistrypriv = &padapter->registrypriv; + /* struct wlan_network *pcur_network = &pmlmepriv->cur_network;; */ + 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_8723A("+rtw_update_ht_cap23a()\n"); + + /* maybe needs check if ap supports rx ampdu. */ + if ((phtpriv->ampdu_enable == false) && (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_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_CAPABILITY_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies)); + if (p && len > 0) { + pht_capie = (struct ieee80211_ht_cap *)(p+2); + max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR); + max_ampdu_sz = 1 << (max_ampdu_sz+3); /* max_ampdu_sz (kbytes); */ + + /* DBG_8723A("rtw_update_ht_cap23a(): max_ampdu_sz =%d\n", max_ampdu_sz); */ + phtpriv->rx_ampdu_maxlen = max_ampdu_sz; + + } + + len = 0; + p = rtw_get_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_ADD_INFO_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies)); + if (p && len>0) + { + pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2); + /* todo: */ + } + + /* update cur_bwmode & cur_ch_offset */ + if ((pregistrypriv->cbw40_enable) && + (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_1R23A[i]; + else + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i]; + } + /* switch to the 40M Hz mode accoring 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 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) + DBG_8723A("%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_cmd23a(struct rtw_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_multicast_ether_addr(pattrib->ra); + + if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod<100)) + return; + + priority = pattrib->priority; + + if (pattrib->psta) + psta = pattrib->psta; + else + { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra); + } + + if (psta == NULL) + { + DBG_8723A("%s, psta == NUL\n", __func__); + return; + } + + if (!(psta->state &_FW_LINKED)) + { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return; + } + + phtpriv = &psta->htpriv; + + if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) + { + issued = (phtpriv->agg_enable_bitmap>>priority)&0x1; + issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1; + + if (0 == issued) + { + DBG_8723A("rtw_issue_addbareq_cmd23a, p =%d\n", priority); + psta->htpriv.candidate_tid_bitmap |= CHKBIT((u8)priority); + rtw_addbareq_cmd23a(padapter, (u8) priority, pattrib->ra); + } + } +} + +inline void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming) +{ + if (to_roaming == 0) + adapter->mlmepriv.to_join = false; + adapter->mlmepriv.to_roaming = to_roaming; +} + +inline u8 rtw_to_roaming(struct rtw_adapter *adapter) +{ + return adapter->mlmepriv.to_roaming; +} + +void rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + spin_lock_bh(&pmlmepriv->lock); + _rtw23a_roaming(padapter, tgt_network); + spin_unlock_bh(&pmlmepriv->lock); +} +void _rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *pnetwork; + int do_join_r; + + if (tgt_network != NULL) + pnetwork = tgt_network; + else + pnetwork = &pmlmepriv->cur_network; + + if (0 < rtw_to_roaming(padapter)) { + DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n", + pnetwork->network.Ssid.ssid, + MAC_ARG(pnetwork->network.MacAddress), + pnetwork->network.Ssid.ssid_len); + memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid, + sizeof(struct cfg80211_ssid)); + + pmlmepriv->assoc_by_bssid = false; + + while(1) { + if (_SUCCESS == (do_join_r = rtw_do_join23a(padapter))) { + break; + } else { + DBG_8723A("roaming do_join return %d\n", do_join_r); + pmlmepriv->to_roaming--; + + if (0 < rtw_to_roaming(padapter)) { + continue; + } else { + DBG_8723A("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__); + rtw_indicate_disconnect23a(padapter); + break; + } + } + } + } +} + +int rtw_linked_check(struct rtw_adapter *padapter) +{ + if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) || + (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))) { + if (padapter->stapriv.asoc_sta_count > 2) + return true; + } else { /* Station mode */ + if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true) + return true; + } + return false; +} diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c new file mode 100644 index 000000000000..75ccdec881e5 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c @@ -0,0 +1,9988 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_MLME_EXT_C_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef CONFIG_8723AU_BT_COEXIST +#include +#endif + +static struct mlme_handler mlme_sta_tbl[]={ + {"OnAssocReq23a", &OnAssocReq23a}, + {"OnAssocRsp23a", &OnAssocRsp23a}, + {"OnReAssocReq", &OnAssocReq23a}, + {"OnReAssocRsp", &OnAssocRsp23a}, + {"OnProbeReq23a", &OnProbeReq23a}, + {"OnProbeRsp23a", &OnProbeRsp23a}, + + /*---------------------------------------------------------- + below 2 are reserved + -----------------------------------------------------------*/ + {"DoReserved23a", &DoReserved23a}, + {"DoReserved23a", &DoReserved23a}, + {"OnBeacon23a", &OnBeacon23a}, + {"OnATIM", &OnAtim23a}, + {"OnDisassoc23a", &OnDisassoc23a}, + {"OnAuth23a", &OnAuth23aClient23a}, + {"OnDeAuth23a", &OnDeAuth23a}, + {"OnAction23a", &OnAction23a}, +}; + +static struct action_handler OnAction23a_tbl[]={ + {WLAN_CATEGORY_SPECTRUM_MGMT, "ACTION_SPECTRUM_MGMT", on_action_spct23a}, + {WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction23a_qos}, + {WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction23a_dls}, + {WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction23a_back23a}, + {WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public23a}, + {WLAN_CATEGORY_HT, "ACTION_HT", &OnAction23a_ht}, + {WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved23a}, + {WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction23a_wmm}, + {WLAN_CATEGORY_VENDOR_SPECIFIC, "ACTION_P2P", &OnAction23a_p2p}, +}; + +static u8 null_addr[ETH_ALEN]= {0, 0, 0, 0, 0, 0}; + +/************************************************** +OUI definitions for the vendor specific IE +***************************************************/ +unsigned char RTW_WPA_OUI23A[] = {0x00, 0x50, 0xf2, 0x01}; +unsigned char WMM_OUI23A[] = {0x00, 0x50, 0xf2, 0x02}; +unsigned char WPS_OUI23A[] = {0x00, 0x50, 0xf2, 0x04}; +unsigned char P2P_OUI23A[] = {0x50, 0x6F, 0x9A, 0x09}; +unsigned char WFD_OUI23A[] = {0x50, 0x6F, 0x9A, 0x0A}; + +unsigned char WMM_INFO_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01}; +unsigned char WMM_PARA_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + +unsigned char WPA_TKIP_CIPHER23A[4] = {0x00, 0x50, 0xf2, 0x02}; +unsigned char RSN_TKIP_CIPHER23A[4] = {0x00, 0x0f, 0xac, 0x02}; + + +/******************************************************** +MCS rate definitions +*********************************************************/ +unsigned char MCS_rate_2R23A[16] = { + 0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; +unsigned char MCS_rate_1R23A[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_5g RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = { + {{}, 0}, /* 0x00, RT_CHANNEL_DOMAIN_5G_NULL */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19}, /* 0x01, RT_CHANNEL_DOMAIN_5G_ETSI1 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24}, /* 0x02, RT_CHANNEL_DOMAIN_5G_ETSI2 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 149, 153, 157, 161, 165}, 22}, /* 0x03, RT_CHANNEL_DOMAIN_5G_ETSI3 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24}, /* 0x04, RT_CHANNEL_DOMAIN_5G_FCC1 */ + {{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9}, /* 0x05, RT_CHANNEL_DOMAIN_5G_FCC2 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13}, /* 0x06, RT_CHANNEL_DOMAIN_5G_FCC3 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161}, 12}, /* 0x07, RT_CHANNEL_DOMAIN_5G_FCC4 */ + {{149, 153, 157, 161, 165}, 5}, /* 0x08, RT_CHANNEL_DOMAIN_5G_FCC5 */ + {{36, 40, 44, 48, 52, 56, 60, 64}, 8}, /* 0x09, RT_CHANNEL_DOMAIN_5G_FCC6 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20}, /* 0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, 165}, 20}, /* 0x0B, RT_CHANNEL_DOMAIN_5G_KCC1 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19}, /* 0x0C, RT_CHANNEL_DOMAIN_5G_MKK1 */ + {{36, 40, 44, 48, 52, 56, 60, 64}, 8}, /* 0x0D, RT_CHANNEL_DOMAIN_5G_MKK2 */ + {{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11}, /* 0x0E, RT_CHANNEL_DOMAIN_5G_MKK3 */ + {{56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 15}, /* 0x0F, RT_CHANNEL_DOMAIN_5G_NCC1 */ + {{56, 60, 64, 149, 153, 157, 161, 165}, 8}, /* 0x10, RT_CHANNEL_DOMAIN_5G_NCC2 */ + + /* Driver self defined for old channel plan Compatible , Remember to modify if have new channel plan definition ===== */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 21}, /* 0x11, RT_CHANNEL_DOMAIN_5G_FCC */ + {{36, 40, 44, 48}, 4}, /* 0x12, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */ + {{36, 40, 44, 48, 149, 153, 157, 161}, 8}, /* 0x13, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */ +}; + +static struct rt_channel_plan_map RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = { + /* 0x00 ~ 0x1F , Old Define ===== */ + {0x02, 0x11}, /* 0x00, RT_CHANNEL_DOMAIN_FCC */ + {0x02, 0x0A}, /* 0x01, RT_CHANNEL_DOMAIN_IC */ + {0x01, 0x01}, /* 0x02, RT_CHANNEL_DOMAIN_ETSI */ + {0x01, 0x00}, /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */ + {0x01, 0x00}, /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */ + {0x03, 0x00}, /* 0x05, RT_CHANNEL_DOMAIN_MKK */ + {0x03, 0x00}, /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */ + {0x01, 0x09}, /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */ + {0x03, 0x09}, /* 0x08, RT_CHANNEL_DOMAIN_TELEC */ + {0x03, 0x00}, /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */ + {0x00, 0x00}, /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */ + {0x02, 0x0F}, /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */ + {0x01, 0x08}, /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */ + {0x02, 0x06}, /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */ + {0x02, 0x0B}, /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */ + {0x02, 0x09}, /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */ + {0x01, 0x01}, /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */ + {0x02, 0x05}, /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */ + {0x01, 0x12}, /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */ + {0x00, 0x04}, /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */ + {0x02, 0x10}, /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */ + {0x00, 0x12}, /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */ + {0x00, 0x13}, /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */ + {0x03, 0x12}, /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */ + {0x05, 0x08}, /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */ + {0x02, 0x08}, /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */ + {0x00, 0x00}, /* 0x1A, */ + {0x00, 0x00}, /* 0x1B, */ + {0x00, 0x00}, /* 0x1C, */ + {0x00, 0x00}, /* 0x1D, */ + {0x00, 0x00}, /* 0x1E, */ + {0x05, 0x04}, /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */ + /* 0x20 ~ 0x7F , New Define ===== */ + {0x00, 0x00}, /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */ + {0x01, 0x00}, /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */ + {0x02, 0x00}, /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */ + {0x03, 0x00}, /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */ + {0x04, 0x00}, /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */ + {0x02, 0x04}, /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */ + {0x00, 0x01}, /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */ + {0x03, 0x0C}, /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */ + {0x00, 0x0B}, /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */ + {0x00, 0x05}, /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */ + {0x00, 0x00}, /* 0x2A, */ + {0x00, 0x00}, /* 0x2B, */ + {0x00, 0x00}, /* 0x2C, */ + {0x00, 0x00}, /* 0x2D, */ + {0x00, 0x00}, /* 0x2E, */ + {0x00, 0x00}, /* 0x2F, */ + {0x00, 0x06}, /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */ + {0x00, 0x07}, /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */ + {0x00, 0x08}, /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */ + {0x00, 0x09}, /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */ + {0x02, 0x0A}, /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */ + {0x00, 0x02}, /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */ + {0x00, 0x03}, /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */ + {0x03, 0x0D}, /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */ + {0x03, 0x0E}, /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */ + {0x02, 0x0F}, /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */ + {0x00, 0x00}, /* 0x3A, */ + {0x00, 0x00}, /* 0x3B, */ + {0x00, 0x00}, /* 0x3C, */ + {0x00, 0x00}, /* 0x3D, */ + {0x00, 0x00}, /* 0x3E, */ + {0x00, 0x00}, /* 0x3F, */ + {0x02, 0x10}, /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */ + {0x03, 0x00}, /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */ +}; + +static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */ + +static struct fwevent wlanevents[] = +{ + {0, rtw_dummy_event_callback23a}, /*0*/ + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, &rtw_survey_event_cb23a}, /*8*/ + {sizeof (struct surveydone_event), &rtw_surveydone_event_callback23a}, /*9*/ + + {0, &rtw23a_joinbss_event_cb}, /*10*/ + {sizeof(struct stassoc_event), &rtw_stassoc_event_callback23a}, + {sizeof(struct stadel_event), &rtw_stadel_event_callback23a}, + {0, &rtw_atimdone_event_callback23a}, + {0, rtw_dummy_event_callback23a}, + {0, NULL}, /*15*/ + {0, NULL}, + {0, NULL}, + {0, NULL}, + {0, rtw23a_fwdbg_event_callback}, + {0, NULL}, /*20*/ + {0, NULL}, + {0, NULL}, + {0, &rtw_cpwm_event_callback23a}, + {0, NULL}, +}; + + +/* + * 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_ch23a(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_ext23a(struct rtw_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, + pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + return _SUCCESS; +} + +static void init_mlme_ext_priv23a_value(struct rtw_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); + /* reset to zero when disconnect at client mode */ + pmlmeext->mgnt_seq = 0; + + 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); + + if (pmlmeext->cur_channel > 14) + pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB; + else + 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 rtw_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 }, + { IEEE80211A, 115, 36, 48, 4, BW20 }, + { IEEE80211A, 116, 36, 44, 8, BW40PLUS }, + { IEEE80211A, 117, 40, 48, 8, BW40MINUS }, + { IEEE80211A, 124, 149, 161, 4, BW20 }, + { IEEE80211A, 125, 149, 169, 4, BW20 }, + { IEEE80211A, 126, 149, 157, 8, BW40PLUS }, + { IEEE80211A, 127, 153, 161, 8, BW40MINUS }, + { -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) && + (o->inc == 8)) + 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 rtw_adapter* padapter, u8 ChannelPlan, + struct rt_channel_info *channel_set) +{ + u8 index, chanset_size = 0; + u8 b5GBand = false, b2_4GBand = false; + u8 Index2G = 0, Index5G = 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_8723A("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 (padapter->registrypriv.wireless_mode & WIRELESS_11A) { + b5GBand = true; + if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan) + Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G; + else + Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G; + } + + if (b2_4GBand) { + for (index = 0; index= 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_WORLD_WIDE_5G == + 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++; + } + } + + if (b5GBand) { + for (index = 0;index= 149) { + channel_set[chanset_size].ChannelNum = + RTW_ChannelPlan5G[Index5G].Channel[index]; + if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == + ChannelPlan) { + /* passive scan for all 5G channels */ + channel_set[chanset_size].ScanType = + SCAN_PASSIVE; + } else + channel_set[chanset_size].ScanType = + SCAN_ACTIVE; + DBG_8723A("%s(): channel_set[%d].ChannelNum = " + "%d\n", __func__, chanset_size, + channel_set[chanset_size].ChannelNum); + chanset_size++; + } + } + } + + return chanset_size; +} + +int init_mlme_ext_priv23a(struct rtw_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_priv23a_value(padapter); + pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq; + + init_mlme_ext_timer23a(padapter); + +#ifdef CONFIG_8723AU_AP_MODE + init_mlme_ap_info23a(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_priv23a (struct mlme_ext_priv *pmlmeext) +{ + struct rtw_adapter *padapter = pmlmeext->padapter; + + if (!padapter) + return; + + if (padapter->bDriverStopped == true) { + del_timer_sync(&pmlmeext->survey_timer); + del_timer_sync(&pmlmeext->link_timer); + /* del_timer_sync(&pmlmeext->ADDBA_timer); */ + } +} + +static void +_mgt_dispatcher23a(struct rtw_adapter *padapter, struct mlme_handler *ptable, + struct recv_frame *precv_frame) +{ + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + + if (ptable->func) { + /* receive the frames that ra(a1) is my address + or ra(a1) is bc address. */ + if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv))&& + !is_broadcast_ether_addr(hdr->addr1)) + return; + + ptable->func(padapter, precv_frame); + } +} + +void mgt_dispatcher23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + int index; + struct mlme_handler *ptable; +#ifdef CONFIG_8723AU_AP_MODE + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#endif /* CONFIG_8723AU_AP_MODE */ + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u16 stype; + struct sta_info *psta; + + if (!ieee80211_is_mgmt(hdr->frame_control)) + return; + + /* receive the frames that ra(a1) is my address or ra(a1) is + bc address. */ + if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv)) && + !is_broadcast_ether_addr(hdr->addr1)) + return; + + ptable = mlme_sta_tbl; + + stype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE; + index = stype >> 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; + + psta = rtw_get_stainfo23a(&padapter->stapriv, hdr->addr2); + + if (psta) { + if (ieee80211_has_retry(hdr->frame_control)) { + if (precv_frame->attrib.seq_num == + psta->RxMgmtFrameSeqNum) { + /* drop the duplicate management frame */ + DBG_8723A("Drop duplicate management frame " + "with seq_num = %d.\n", + precv_frame->attrib.seq_num); + return; + } + } + psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num; + } + +#ifdef CONFIG_8723AU_AP_MODE + switch (stype) + { + case IEEE80211_STYPE_AUTH: + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) + ptable->func = &OnAuth23a; + else + ptable->func = &OnAuth23aClient23a; + /* pass through */ + case IEEE80211_STYPE_ASSOC_REQ: + case IEEE80211_STYPE_REASSOC_REQ: + _mgt_dispatcher23a(padapter, ptable, precv_frame); + break; + case IEEE80211_STYPE_PROBE_REQ: + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) + _mgt_dispatcher23a(padapter, ptable, precv_frame); + else + _mgt_dispatcher23a(padapter, ptable, precv_frame); + break; + case IEEE80211_STYPE_BEACON: + _mgt_dispatcher23a(padapter, ptable, precv_frame); + break; + case IEEE80211_STYPE_ACTION: + /* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */ + _mgt_dispatcher23a(padapter, ptable, precv_frame); + break; + default: + _mgt_dispatcher23a(padapter, ptable, precv_frame); + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) + rtw_hostapd_mlme_rx23a(padapter, precv_frame); + break; + } +#else + _mgt_dispatcher23a(padapter, ptable, precv_frame); +#endif +} + +#ifdef CONFIG_8723AU_P2P +static u32 p2p_listen_state_process(struct rtw_adapter *padapter, + unsigned char *da) +{ + bool response = true; + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == false || + padapter->mlmepriv.wps_probe_resp_ie == NULL || + padapter->mlmepriv.p2p_probe_resp_ie == NULL) { + DBG_8723A("DON'T issue_probersp23a_p2p23a: p2p_enabled:%d, " + "wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n", + wdev_to_priv(padapter->rtw_wdev)->p2p_enabled, + padapter->mlmepriv.wps_probe_resp_ie, + padapter->mlmepriv.p2p_probe_resp_ie); + response = false; + } + + if (response == true) + issue_probersp23a_p2p23a(padapter, da); + + return _SUCCESS; +} +#endif /* CONFIG_8723AU_P2P */ + +/**************************************************************************** + +Following are the callback functions for each subtype of the management frames + +*****************************************************************************/ + +unsigned int OnProbeReq23a(struct rtw_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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint len = skb->len; + u8 is_valid_p2p_probereq = false; + +#ifdef CONFIG_8723AU_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 */ + + /* 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) { + if ((is_valid_p2p_probereq = + process_probe_req_p2p_ie23a(pwdinfo, pframe, + len)) == true) { + if (rtw_p2p_chk_role(pwdinfo, + P2P_ROLE_DEVICE)) { + u8 *sa = ieee80211_get_SA(hdr); + p2p_listen_state_process(padapter, sa); + return _SUCCESS; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + goto _continue; + } + } + } + } + +_continue: +#endif /* CONFIG_8723AU_P2P */ + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + return _SUCCESS; + } + + if (check_fwstate(pmlmepriv, _FW_LINKED) == false && + check_fwstate(pmlmepriv, + WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == false) { + return _SUCCESS; + } + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen, + len - sizeof(struct ieee80211_hdr_3addr) - + _PROBEREQ_IE_OFFSET_); + + /* check (wildcard) SSID */ + if (p) { + if (is_valid_p2p_probereq == true) { + goto _issue_probersp23a; + } + + if ((ielen != 0 && + memcmp((void *)(p+2), cur->Ssid.ssid, + cur->Ssid.ssid_len)) || + (ielen == 0 && pmlmeinfo->hidden_ssid_mode)) { + return _SUCCESS; + } + +_issue_probersp23a: + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true && + pmlmepriv->cur_network.join_res == true) { + /* DBG_8723A("+issue_probersp23a during ap mode\n"); */ + issue_probersp23a(padapter, ieee80211_get_SA(hdr), + is_valid_p2p_probereq); + } + } + + return _SUCCESS; +} + +unsigned int OnProbeRsp23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif + +#ifdef CONFIG_8723AU_P2P + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) { + if (pwdinfo->tx_prov_disc_info.benable == true) { + if (ether_addr_equal(pwdinfo->tx_prov_disc_info.peerIFAddr, + hdr->addr2)) { + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + pwdinfo->tx_prov_disc_info.benable = false; + issue_p2p_provision_request23a(padapter, + pwdinfo->tx_prov_disc_info.ssid.ssid, + pwdinfo->tx_prov_disc_info.ssid.ssid_len, + 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_request23a(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 == true) { + DBG_8723A("[%s] P2P State is GONEGO ING!\n", __func__); + if (ether_addr_equal(pwdinfo->nego_req_info.peerDevAddr, + hdr->addr2)) { + pwdinfo->nego_req_info.benable = false; + issue_p2p_GO_request23a(padapter, pwdinfo->nego_req_info.peerDevAddr); + } + } + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) { + if (pwdinfo->invitereq_info.benable == true) { + DBG_8723A("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__); + if (ether_addr_equal( + pwdinfo->invitereq_info.peer_macaddr, + hdr->addr2)) { + pwdinfo->invitereq_info.benable = false; + issue_p2p_invitation_request23a(padapter, pwdinfo->invitereq_info.peer_macaddr); + } + } + } +#endif + + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + report_survey_event23a(padapter, precv_frame); + return _SUCCESS; + } + + return _SUCCESS; +} + +unsigned int OnBeacon23a(struct rtw_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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint len = skb->len; + struct wlan_bssid_ex *pbss; + int ret = _SUCCESS; + u8 *p = NULL; + u32 ielen = 0; + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, + len - sizeof(struct ieee80211_hdr_3addr) - + _BEACON_IE_OFFSET_); + if ((p != NULL) && (ielen > 0)) { + if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D)) { + /* Invalid value 0x2D is detected in Extended Supported + * Rates (ESR) IE. Try to fix the IE length to avoid + * failed Beacon parsing. + */ + DBG_8723A("[WIFIDBG] Error in ESR IE is detected in " + "Beacon of BSSID: %pM. Fix the length of " + "ESR IE to avoid failed Beacon parsing.\n", + hdr->addr3); + *(p + 1) = ielen - 1; + } + } + + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + report_survey_event23a(padapter, precv_frame); + return _SUCCESS; + } + + if (ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network))){ + if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) { + /* we should update current network before auth, + or some IE is wrong */ + pbss = (struct wlan_bssid_ex *) + kmalloc(sizeof(struct wlan_bssid_ex), + GFP_ATOMIC); + if (pbss) { + if (collect_bss_info23a(padapter, precv_frame, + pbss) == _SUCCESS) { + update_network23a(&pmlmepriv->cur_network.network, pbss, padapter, true); + rtw_get_bcn_info23a(&pmlmepriv->cur_network); + } + kfree(pbss); + } + + /* check the vendor of the assoc AP */ + pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pframe + sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct ieee80211_hdr_3addr)); + + /* update TSF Value */ + update_TSF23a(pmlmeext, pframe, len); + + /* start auth */ + start_clnt_auth23a(padapter); + + return _SUCCESS; + } + + if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && + (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + psta = rtw_get_stainfo23a(pstapriv, hdr->addr2); + if (psta) { + ret = rtw_check_bcn_info23a(padapter, pframe, + len); + if (!ret) { + DBG_8723A_LEVEL(_drv_always_, + "ap has changed, " + "disconnect now\n"); + receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535); + 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) { + /* DBG_8723A("update_bcn_info\n"); */ + update_beacon23a_info(padapter, pframe, + len, psta); + } + +#ifdef CONFIG_8723AU_P2P + process_p2p_ps_ie23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr)), (len - sizeof(struct ieee80211_hdr_3addr))); +#endif /* CONFIG_8723AU_P2P */ + } + } else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) { + psta = rtw_get_stainfo23a(pstapriv, hdr->addr2); + if (psta) { + /* 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) { + /* DBG_8723A("update_bcn_info\n"); */ + update_beacon23a_info(padapter, pframe, + len, psta); + } + } else { + /* allocate a new CAM entry for IBSS station */ + cam_idx = allocate_fw_sta_entry23a(padapter); + if (cam_idx == NUM_STA) + goto _END_ONBEACON_; + + /* get supported rate */ + if (update_sta_support_rate23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_), (len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) { + pmlmeinfo->FW_sta_info[cam_idx].status = 0; + goto _END_ONBEACON_; + } + + /* update TSF Value */ + update_TSF23a(pmlmeext, pframe, len); + + /* report sta add event */ + report_add_sta_event23a(padapter, hdr->addr2, + cam_idx); + } + } + } + +_END_ONBEACON_: + + return _SUCCESS; +} + +unsigned int OnAuth23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_AP_MODE + unsigned int auth_mode, seq, ie_len; + 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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint len = skb->len; + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + DBG_8723A("+OnAuth23a\n"); + + sa = hdr->addr2; + + auth_mode = psecuritypriv->dot11AuthAlgrthm; + seq = cpu_to_le16(*(u16*)((unsigned long)pframe + + sizeof(struct ieee80211_hdr_3addr) + 2)); + algorithm = cpu_to_le16(*(u16*)((unsigned long)pframe + + sizeof(struct ieee80211_hdr_3addr))); + + DBG_8723A("auth alg =%x, seq =%X\n", algorithm, seq); + + if (auth_mode == 2 && + psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ && + psecuritypriv->dot11PrivacyAlgrthm != _WEP104_) + auth_mode = 0; + + /* rx a shared-key auth but shared not enabled, or */ + /* rx a open-system auth but shared-key is enabled */ + if ((algorithm > 0 && auth_mode == 0) || + (algorithm == 0 && auth_mode == 1)) { + DBG_8723A("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 = WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG; + + goto auth_fail; + } + + if (rtw_access_ctrl23a(padapter, sa) == false) { + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + goto auth_fail; + } + + pstat = rtw_get_stainfo23a(pstapriv, sa); + if (!pstat) { + /* allocate a new one */ + DBG_8723A("going to alloc stainfo for sa ="MAC_FMT"\n", + MAC_ARG(sa)); + pstat = rtw_alloc_stainfo23a(pstapriv, sa); + if (!pstat) { + DBG_8723A(" Exceed the upper limit of supported " + "clients...\n"); + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + goto auth_fail; + } + + pstat->state = WIFI_FW_AUTH_NULL; + pstat->auth_seq = 0; + + /* pstat->flags = 0; */ + /* pstat->capability = 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--; + if (pstat->expire_to > 0) + { + /* TODO: STA re_auth within expire_to */ + } + } + 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_8723A("(1)auth rejected because out of seq [rx_seq =%d, " + "exp_seq =%d]!\n", seq, pstat->auth_seq+1); + status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION; + 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_8723A("(2)auth rejected because out of seq " + "[rx_seq =%d, exp_seq =%d]!\n", + seq, pstat->auth_seq+1); + status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION; + 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_8723A("checking for challenging txt...\n"); + + p = rtw_get_ie23a(pframe + + sizeof(struct ieee80211_hdr_3addr) + + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, + (int *)&ie_len, len - + sizeof(struct ieee80211_hdr_3addr) - + _AUTH_IE_OFFSET_ - 4); + + if ((p == NULL) || (ie_len<= 0)) { + DBG_8723A("auth rejected because challenge " + "failure!(1)\n"); + status = WLAN_STATUS_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_8723A("auth rejected because challenge " + "failure!\n"); + status = WLAN_STATUS_CHALLENGE_FAIL; + goto auth_fail; + } + } else { + DBG_8723A("(3)auth rejected because out of seq " + "[rx_seq =%d, exp_seq =%d]!\n", + seq, pstat->auth_seq+1); + status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION; + goto auth_fail; + } + } + + /* Now, we are going to issue_auth23a... */ + pstat->auth_seq = seq + 1; + + issue_auth23a(padapter, pstat, (unsigned short)WLAN_STATUS_SUCCESS); + + if (pstat->state & WIFI_FW_AUTH_SUCCESS) + pstat->auth_seq = 0; + + return _SUCCESS; + +auth_fail: + + if (pstat) + rtw_free_stainfo23a(padapter, pstat); + + pstat = &stat; + memset((char *)pstat, '\0', sizeof(stat)); + pstat->auth_seq = 2; + memcpy(pstat->hwaddr, sa, 6); + + issue_auth23a(padapter, pstat, (unsigned short)status); + +#endif + return _FAIL; +} + +unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + unsigned int seq, len, status, algthm, offset; + unsigned char *p; + unsigned int go2asoc = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint pkt_len = skb->len; + + DBG_8723A("%s\n", __func__); + + /* check A1 matches or not */ + if (!ether_addr_equal(myid(&padapter->eeprompriv), + ieee80211_get_DA(hdr))) + return _SUCCESS; + + if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE)) + return _SUCCESS; + + offset = ieee80211_has_protected(hdr->frame_control) ? 4: 0; + + algthm = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset)); + seq = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 2)); + status = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 4)); + + if (status != 0) + { + DBG_8723A("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; + /* pmlmeinfo->reauth_count = 0; */ + } + + set_link_timer(pmlmeext, 1); + goto authclnt_fail; + } + + if (seq == 2) + { + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) + { + /* legendary shared system */ + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len, + pkt_len - sizeof(struct ieee80211_hdr_3addr) - _AUTH_IE_OFFSET_); + + if (p == NULL) + { + /* DBG_8723A("marc: no challenge text?\n"); */ + goto authclnt_fail; + } + + memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len); + pmlmeinfo->auth_seq = 3; + issue_auth23a(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 */ + /* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n", seq); */ + goto authclnt_fail; + } + + if (go2asoc) + { + DBG_8723A_LEVEL(_drv_always_, "auth success, start assoc\n"); + start_clnt_assoc23a(padapter); + return _SUCCESS; + } + +authclnt_fail: + + /* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */ + + return _FAIL; +} + +unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_AP_MODE + u16 capab_info, listen_interval; + 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 = WLAN_STATUS_SUCCESS; + unsigned short ie_offset; + 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; + struct sk_buff *skb = precv_frame->pkt; + u8 *pframe = skb->data; + uint pkt_len = skb->len; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u16 frame_control; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 p2p_status_code = P2P_STATUS_SUCCESS; + u8 *p2pie; + u32 p2pielen = 0; + u8 wfd_ie[ 128 ] = { 0x00 }; + u32 wfd_ielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + frame_control = hdr->frame_control; + if (ieee80211_is_assoc_req(frame_control)) { + reassoc = 0; + ie_offset = _ASOCREQ_IE_OFFSET_; + } else { /* WIFI_REASSOCREQ */ + reassoc = 1; + ie_offset = _REASOCREQ_IE_OFFSET_; + } + + if (pkt_len < sizeof(struct ieee80211_hdr_3addr) + ie_offset) { + DBG_8723A("handle_assoc(reassoc =%d) - too short payload (len =%lu)" + "\n", reassoc, (unsigned long)pkt_len); + return _FAIL; + } + + pstat = rtw_get_stainfo23a(pstapriv, hdr->addr2); + if (!pstat) { + status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA; + goto asoc_class2_error; + } + + capab_info = RTW_GET_LE16(pframe + sizeof(struct ieee80211_hdr_3addr)); + /* capab_info = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr))); */ + /* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)+2)); */ + listen_interval = RTW_GET_LE16(pframe + sizeof(struct ieee80211_hdr_3addr)+2); + + left = pkt_len - (sizeof(struct ieee80211_hdr_3addr) + ie_offset); + pos = pframe + (sizeof(struct ieee80211_hdr_3addr) + ie_offset); + + DBG_8723A("%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 = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA; + 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_elems23a(pos, left, &elems, 1) == ParseFailed || + !elems.ssid) { + DBG_8723A("STA " MAC_FMT " sent invalid association request\n", + MAC_ARG(pstat->hwaddr)); + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + goto OnAssocReq23aFail; + } + + /* now we should check all the fields... */ + /* checking SSID */ + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SSID_IE_, &ie_len, + pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset); + if (p == NULL) + { + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + } + + if (ie_len == 0) /* broadcast ssid, however it is not allowed in assocreq */ + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + else { + /* check if ssid match */ + if (memcmp((void *)(p+2), cur->Ssid.ssid, cur->Ssid.ssid_len)) + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + + if (ie_len != cur->Ssid.ssid_len) + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + } + + if (WLAN_STATUS_SUCCESS != status) + goto OnAssocReq23aFail; + + /* check if the supported rate is ok */ + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset); + if (p == NULL) { + DBG_8723A("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 = WLAN_STATUS_UNSPECIFIED_FAILURE; + goto OnAssocReq23aFail; + } else { + memcpy(supportRate, p+2, ie_len); + supportRateNum = ie_len; + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len, + pkt_len - sizeof(struct ieee80211_hdr_3addr) - 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); + Update23aTblForSoftAP(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_ie23a(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_REASON_INVALID_GROUP_CIPHER; + + if (!pstat->wpa2_pairwise_cipher) + status = WLAN_REASON_INVALID_PAIRWISE_CIPHER; + } 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_ie23a(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_INVALID_GROUP_CIPHER; + + if (!pstat->wpa_pairwise_cipher) + status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER; + + } else { + status = WLAN_STATUS_INVALID_IE; + } + + } else { + wpa_ie = NULL; + wpa_ie_len = 0; + } + + if (WLAN_STATUS_SUCCESS != status) + goto OnAssocReq23aFail; + + pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS); + if (wpa_ie == NULL) { + if (elems.wps_ie) { + DBG_8723A("STA included WPS IE in " + "(Re)Association Request - assume WPS is " + "used\n"); + pstat->flags |= WLAN_STA_WPS; + } else { + DBG_8723A("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_content23a(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, + WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL); + + if (!selected_registrar) { + DBG_8723A("selected_registrar is false , or AP is not ready to do WPS\n"); + + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + + goto OnAssocReq23aFail; + } + } + } + } else { + int copy_len; + + if (psecuritypriv->wpa_psk == 0) { + DBG_8723A("STA " MAC_FMT ": WPA/RSN IE in association " + "request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr)); + + status = WLAN_STATUS_INVALID_IE; + + goto OnAssocReq23aFail; + } + + if (elems.wps_ie) { + DBG_8723A("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 + sizeof(struct ieee80211_hdr_3addr) + ie_offset; ie_len = 0; + for (;;) + { + p = rtw_get_ie23a(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - 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 == false) && (pstat->flags&WLAN_STA_HT)) + { + status = WLAN_STATUS_UNSPECIFIED_FAILURE; + goto OnAssocReq23aFail; + } + + if ((pstat->flags & WLAN_STA_HT) && + ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) || + (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) + { + DBG_8723A("HT: " MAC_FMT " tried to " + "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr)); + + /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */ + /* goto OnAssocReq23aFail; */ + } + + /* */ + 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 != WLAN_STATUS_SUCCESS) + goto OnAssocReq23aFail; + +#ifdef CONFIG_8723AU_P2P + pstat->is_p2p_device = false; + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, NULL, &p2pielen))) + { + pstat->is_p2p_device = true; + if ((p2p_status_code = (u8)process_assoc_req_p2p_ie23a(pwdinfo, pframe, pkt_len, pstat))>0) + { + pstat->p2p_status_code = p2p_status_code; + status = WLAN_STATUS_CAPS_UNSUPPORTED; + goto OnAssocReq23aFail; + } + } +#ifdef CONFIG_8723AU_P2P + if (rtw_get_wfd_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, wfd_ie, &wfd_ielen)) + { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] WFD IE Found!!\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) + { + pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + } + } +#endif + } + pstat->p2p_status_code = p2p_status_code; +#endif /* CONFIG_8723AU_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_8723A(" 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) + pstat->aid = NUM_STA; + if (pstat->aid > pstapriv->max_num_sta) { + + pstat->aid = 0; + + DBG_8723A(" no room for more AIDs\n"); + + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + + goto OnAssocReq23aFail; + + } else { + pstapriv->sta_aid[pstat->aid - 1] = pstat; + DBG_8723A("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) && + (WLAN_STATUS_SUCCESS == status)) { +#ifdef CONFIG_8723AU_AP_MODE + /* 1 bss_cap_update & sta_info_update23a */ + bss_cap_update_on_sta_join23a(padapter, pstat); + sta_info_update23a(padapter, pstat); + + /* issue assoc rsp before notify station join event. */ + if (ieee80211_is_assoc_req(frame_control)) + issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP); + + /* 2 - report to upper layer */ + DBG_8723A("indicate_sta_join_event to upper layer - hostapd\n"); + rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len); + + /* 3-(1) report sta add event */ + report_add_sta_event23a(padapter, pstat->hwaddr, pstat->aid); +#endif + } + + return _SUCCESS; + +asoc_class2_error: + +#ifdef CONFIG_8723AU_AP_MODE + issue_deauth23a(padapter, hdr->addr2, status); +#endif + + return _FAIL; + +OnAssocReq23aFail: + +#ifdef CONFIG_8723AU_AP_MODE + pstat->aid = 0; + if (ieee80211_is_assoc_req(frame_control)) + issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP); +#endif + +#endif /* CONFIG_8723AU_AP_MODE */ + + return _FAIL; +} + +unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ + uint i; + int res; + unsigned short status; + struct ndis_802_11_var_ies *pIE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint pkt_len = skb->len; + + DBG_8723A("%s\n", __func__); + + /* check A1 matches or not */ + if (!ether_addr_equal(myid(&padapter->eeprompriv), + ieee80211_get_DA(hdr))) + return _SUCCESS; + + if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE))) + return _SUCCESS; + + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + return _SUCCESS; + + del_timer_sync(&pmlmeext->link_timer); + + /* status */ + if ((status = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 2))) > 0) + { + DBG_8723A("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(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr))); + + /* set slot time */ + pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20; + + /* AID */ + res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 4))&0x3fff); + + /* 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 + sizeof(struct ieee80211_hdr_3addr)); i < pkt_len;) { + pIE = (struct ndis_802_11_var_ies *)(pframe + i); + + switch (pIE->ElementID) + { + case _VENDOR_SPECIFIC_IE_: + if (!memcmp(pIE->data, WMM_PARA_OUI23A, 6))/* WMM */ + WMM_param_handler23a(padapter, pIE); +#if defined(CONFIG_8723AU_P2P) + else if (!memcmp(pIE->data, WFD_OUI23A, 4)) { /* WFD */ + DBG_8723A("[%s] Found WFD IE\n", __func__); + WFD_info_handler(padapter, pIE); + } +#endif + break; + + case _HT_CAPABILITY_IE_: /* HT caps */ + HT_caps_handler23a(padapter, pIE); + break; + + case _HT_EXTRA_INFO_IE_: /* HT info */ + HT_info_handler23a(padapter, pIE); + break; + + case _ERPINFO_IE_: + ERP_IE_handler23a(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 */ + UpdateBrateTbl23a(padapter, pmlmeinfo->network.SupportedRates); + +report_assoc_result: + pmlmepriv->assoc_rsp_len = 0; + if (res > 0) { + kfree(pmlmepriv->assoc_rsp); + pmlmepriv->assoc_rsp = kmalloc(pkt_len, GFP_ATOMIC); + if (pmlmepriv->assoc_rsp) { + memcpy(pmlmepriv->assoc_rsp, pframe, pkt_len); + pmlmepriv->assoc_rsp_len = pkt_len; + } + } else + kfree(pmlmepriv->assoc_rsp); + + report_join_res23a(padapter, res); + + return _SUCCESS; +} + +unsigned int OnDeAuth23a(struct rtw_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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_P2P */ + + /* check A3 */ + if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network))) + return _SUCCESS; + +#ifdef CONFIG_8723AU_P2P + if (pwdinfo->rx_invitereq_info.scan_op_ch_only) { + mod_timer(&pwdinfo->reset_ch_sitesurvey, + jiffies + msecs_to_jiffies(10)); + } +#endif /* CONFIG_8723AU_P2P */ + + reason = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr))); + + DBG_8723A("%s Reason code(%d)\n", __func__, reason); + +#ifdef CONFIG_8723AU_AP_MODE + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_8723A_LEVEL(_drv_always_, "ap recv deauth reason code(%d) " + "sta:%pM\n", reason, hdr->addr2); + + psta = rtw_get_stainfo23a(pstapriv, hdr->addr2); + 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_sta23a(padapter, psta, + false, reason); + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + associated_clients_update23a(padapter, updated); + } + + return _SUCCESS; + } + else +#endif + { + DBG_8723A_LEVEL(_drv_always_, "sta recv deauth reason code(%d) " + "sta:%pM\n", reason, hdr->addr3); + + receive_disconnect23a(padapter, hdr->addr3, reason); + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = false; + return _SUCCESS; +} + +unsigned int OnDisassoc23a(struct rtw_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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_P2P */ + + /* check A3 */ + if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network))) + return _SUCCESS; + +#ifdef CONFIG_8723AU_P2P + if (pwdinfo->rx_invitereq_info.scan_op_ch_only) + { + mod_timer(&pwdinfo->reset_ch_sitesurvey, + jiffies + msecs_to_jiffies(10)); + } +#endif /* CONFIG_8723AU_P2P */ + + reason = le16_to_cpu(*(unsigned short *) + (pframe + sizeof(struct ieee80211_hdr_3addr))); + + DBG_8723A("%s Reason code(%d)\n", __func__, reason); + +#ifdef CONFIG_8723AU_AP_MODE + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d)" + " sta:%pM\n", reason, hdr->addr2); + + psta = rtw_get_stainfo23a(pstapriv, hdr->addr2); + 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_sta23a(padapter, psta, + false, reason); + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + associated_clients_update23a(padapter, updated); + } + + return _SUCCESS; + } + else +#endif + { + DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason " + "code(%d) sta:%pM\n", reason, hdr->addr3); + + receive_disconnect23a(padapter, hdr->addr3, reason); + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = false; + return _SUCCESS; +} + +unsigned int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ + DBG_8723A("%s\n", __func__); + return _SUCCESS; +} + +unsigned int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ + return _FAIL; +} + +unsigned int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction23a_back23a(struct rtw_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; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* check RA matches or not */ + if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1)) + return _SUCCESS; + + DBG_8723A("%s\n", __func__); + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) + return _SUCCESS; + + addr = hdr->addr2; + psta = rtw_get_stainfo23a(pstapriv, addr); + + if (!psta) + return _SUCCESS; + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + category = frame_body[0]; + if (category == WLAN_CATEGORY_BACK) { /* representing Block Ack */ + if (!pmlmeinfo->HT_enable) + return _SUCCESS; + action = frame_body[1]; + DBG_8723A("%s, action =%d\n", __func__, action); + switch (action) { + case WLAN_ACTION_ADDBA_REQ: /* ADDBA request */ + memcpy(&pmlmeinfo->ADDBA_req, &frame_body[2], + sizeof(struct ADDBA_request)); + process_addba_req23a(padapter, + (u8 *)&pmlmeinfo->ADDBA_req, addr); + if (pmlmeinfo->bAcceptAddbaReq == true) + issue_action_BA23a(padapter, addr, + WLAN_ACTION_ADDBA_RESP, 0); + else { + /* reject ADDBA Req */ + issue_action_BA23a(padapter, addr, + WLAN_ACTION_ADDBA_RESP, 37); + } + break; + case WLAN_ACTION_ADDBA_RESP: /* ADDBA response */ + status = RTW_GET_LE16(&frame_body[3]); + tid = ((frame_body[5] >> 2) & 0x7); + if (status == 0) { /* successful */ + DBG_8723A("agg_enable for TID =%d\n", tid); + psta->htpriv.agg_enable_bitmap |= 1 << tid; + psta->htpriv.candidate_tid_bitmap &= + ~CHKBIT(tid); + } else + psta->htpriv.agg_enable_bitmap &= ~CHKBIT(tid); + break; + + case 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 = frame_body[4] | (frame_body[5] << 8); */ + reason_code = RTW_GET_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_8723A("%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_8723AU_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_request23a(struct rtw_adapter *padapter, u8* raddr) +{ + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 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; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + DBG_8723A("[%s] In\n", __func__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pwdinfo->negotiation_dialog_token = 1; /*Initialize the dialog value*/ + pframe = rtw_set_fixed_ie23a(pframe, 1, + &pwdinfo->negotiation_dialog_token, + &pattrib->pktlen); + + /* WPS Section */ + wpsielen = 0; + /* WPS OUI */ + *(u32*) (wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Device Password ID */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + + if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) + { + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC); + } + else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN) + { + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); + } + else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) + { + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC); + } + + wpsielen += 2; + + pframe = rtw_set_ie23a(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: */ + *(u16*) (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: */ + *(u16*) (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: */ + *(u16*) (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: */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Intended P2P Interface Address */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR; + + /* Length: */ + *(u16*) (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); + + *(u16*) (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) */ + *(u16*) (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. */ + + *(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (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 */ + if (pwdinfo->operating_channel <= 14) + { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } + else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) + { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } + else + { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */ + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static void issue_p2p_GO_response(struct rtw_adapter *padapter, u8* raddr, u8* frame_body, uint len, u8 result) +{ + + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 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; + uint wps_devicepassword_id_len = 0; + u16 len_channellist_attr = 0; + int i, j; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + DBG_8723A("[%s] In, result = %d\n", __func__, result); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + /* The Dialog Token of provisioning discovery request frame. */ + pwdinfo->negotiation_dialog_token = frame_body[7]; + pframe = rtw_set_fixed_ie23a(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_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen); + rtw_get_wps_attr_content23a(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, + (u8 *)&wps_devicepassword_id, + &wps_devicepassword_id_len); + wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id); + + memset(wpsie, 0x00, 255); + wpsielen = 0; + + /* WPS Section */ + wpsielen = 0; + /* WPS OUI */ + *(u32*) (wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Device Password ID */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + if (wps_devicepassword_id == WPS_DPID_USER_SPEC) { + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); + } else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) { + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC); + } else { + *(u16*) (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_ie23a(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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = result; + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16*) (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 negotation + 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: */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; + /* 2 seconds needed to be the P2P Client */ + p2pie[p2pielen++] = 200; + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16*) (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 */ + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->operating_channel >= 36) && + (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + /* operating channel number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; + + /* Intended P2P Interface Address */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR; + + /* Length: */ + *(u16*) (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); + + *(u16*) (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 */ + + 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) */ + *(u16*) (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. */ + + *(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (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_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, + (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static void issue_p2p_GO_confirm(struct rtw_adapter *padapter, u8* raddr, + u8 result) +{ + + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + DBG_8723A("[%s] In\n", __func__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, + &pwdinfo->negotiation_dialog_token, + &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 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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = result; + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (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)) { + if (pwdinfo->peer_operating_ch <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->peer_operating_ch >= 36) && + (pwdinfo->peer_operating_ch <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + p2pie[p2pielen++] = pwdinfo->peer_operating_ch; + } else { + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } + else if ((pwdinfo->operating_channel >= 36) && + (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + /* Use the listen channel as the operating channel */ + p2pie[p2pielen++] = pwdinfo->operating_channel; + } + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (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_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, + (unsigned char *)p2pie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +void issue_p2p_invitation_request23a(struct rtw_adapter *padapter, u8* raddr) +{ + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 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; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + int i, j; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, raddr); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; + /* 2 seconds needed to be the P2P Client */ + p2pie[p2pielen++] = 200; + + /* Invitation Flags */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (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 */ + if (pwdinfo->invitereq_info.operating_ch <= 14) + p2pie[p2pielen++] = 0x51; + else if ((pwdinfo->invitereq_info.operating_ch >= 36) && + (pwdinfo->invitereq_info.operating_ch <= 48)) + p2pie[p2pielen++] = 0x73; + else + p2pie[p2pielen++] = 0x7c; + + /* Channel Number */ + /* operating channel number */ + p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch; + + if (ether_addr_equal(myid(&padapter->eeprompriv), + pwdinfo->invitereq_info.go_bssid)) { + /* P2P Group BSSID */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID; + + /* Length: */ + *(u16*) (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); + + *(u16*) (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 */ + 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: */ + *(u16*) (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) */ + *(u16*) (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. */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16*) (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_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, + (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +void issue_p2p_invitation_response23a(struct rtw_adapter *padapter, u8 *raddr, + u8 dialogToken, u8 status_code) +{ + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_INVIT_RESP; + u8 p2pie[ 255 ] = { 0x00 }; + u8 p2pielen = 0; + u16 len_channellist_attr = 0; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + int i, j; + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, raddr); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(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: */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; + /* 2 seconds needed to be the P2P Client */ + p2pie[p2pielen++] = 200; + + 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: */ + *(u16*) (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 */ + /* Copy from SD7 */ + p2pie[p2pielen++] = 0x51; + + /* Channel Number */ + /* operating channel number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; + + /* P2P Group BSSID */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID; + + /* Length: */ + *(u16*) (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); + + *(u16*) (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 */ + 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_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, + (unsigned char *)p2pie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +void issue_p2p_provision_request23a(struct rtw_adapter *padapter, u8 *pssid, + u8 ussidlen, u8 *pdev_raddr) +{ + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u8 dialogToken = 1; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PROVISION_DISC_REQ; + u8 wpsie[100] = { 0x00 }; + u8 wpsielen = 0; + u32 p2pielen = 0; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + DBG_8723A("[%s] In\n", __func__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, pdev_raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, pdev_raddr); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + p2pielen = build_prov_disc_request_p2p_ie23a(pwdinfo, pframe, pssid, + ussidlen, pdev_raddr); + + pframe += p2pielen; + pattrib->pktlen += p2pielen; + + wpsielen = 0; + /* WPS OUI */ + *(u32*) (wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Config Method */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request); + wpsielen += 2; + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, + (unsigned char *) wpsie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static u8 is_matched_in_profilelist(u8 *peermacaddr, + struct profile_info *profileinfo) +{ + u8 i, match_result = 0; + + DBG_8723A("[%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_8723A("[%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 (ether_addr_equal(peermacaddr, profileinfo->peermac)) { + match_result = 1; + DBG_8723A("[%s] Match!\n", __func__); + break; + } + } + + return match_result; +} + +void issue_probersp23a_p2p23a(struct rtw_adapter *padapter, unsigned char *da) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u16 beacon_interval = 100; + u16 capInfo = 0; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 wpsie[255] = { 0x00 }; + u32 wpsielen = 0, p2pielen = 0; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + struct cfg80211_wifidirect_info *pcfg80211_wdinfo = + &padapter->cfg80211_wdinfo; + struct ieee80211_channel *ieee_ch = + &pcfg80211_wdinfo->remain_on_ch_channel; + u8 listen_channel = + (u8)ieee80211_frequency_to_channel(ieee_ch->center_freq); + + /* DBG_8723A("%s\n", __func__); */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&padapter->eeprompriv); + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr2, mac); + + /* Use the device address for BSSID field. */ + ether_addr_copy(pwlanhdr->addr3, mac); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(fctrl, WIFI_PROBERSP); + + pattrib->hdrlen = sizeof(struct 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_ie23a(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_ie23a(pframe, _SUPPORTEDRATES_IE_, 8, + pwdinfo->support_rate, &pattrib->pktlen); + + /* DS parameter set */ + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled && + listen_channel != 0) { + pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *) + &listen_channel, &pattrib->pktlen); + } else { + pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *) + &pwdinfo->listen_channel, + &pattrib->pktlen); + } + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + if (pmlmepriv->wps_probe_resp_ie && + pmlmepriv->p2p_probe_resp_ie) { + /* WPS IE */ + memcpy(pframe, pmlmepriv->wps_probe_resp_ie, + pmlmepriv->wps_probe_resp_ie_len); + pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len; + pframe += pmlmepriv->wps_probe_resp_ie_len; + + /* P2P IE */ + memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, + pmlmepriv->p2p_probe_resp_ie_len); + pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len; + pframe += pmlmepriv->p2p_probe_resp_ie_len; + } + } else { + + /* 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 */ + *(u32*) (wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* WiFi Simple Config State */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG; + + /* Response Type */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X; + + /* UUID-E */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010); + wpsielen += 2; + + /* Value: */ + memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN); + wpsielen += 0x10; + + /* Manufacturer */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0007); + wpsielen += 2; + + /* Value: */ + memcpy(wpsie + wpsielen, "Realtek", 7); + wpsielen += 7; + + /* Model Name */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0006); + wpsielen += 2; + + /* Value: */ + memcpy(wpsie + wpsielen, "8192CU", 6); + wpsielen += 6; + + /* Model Number */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[ wpsielen++ ] = 0x31; /* character 1 */ + + /* Serial Number */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_SERIAL_NUMBER); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(ETH_ALEN); + wpsielen += 2; + + /* Value: */ + memcpy(wpsie + wpsielen, "123456", ETH_ALEN); + wpsielen += ETH_ALEN; + + /* Primary Device Type */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008); + wpsielen += 2; + + /* Value: */ + /* Category ID */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + wpsielen += 2; + + /* OUI */ + *(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* Sub Category ID */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + wpsielen += 2; + + /* Device Name */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + wpsielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(pwdinfo->supported_wps_cm); + wpsielen += 2; + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, + (unsigned char *)wpsie, + &pattrib->pktlen); + + p2pielen = build_probe_resp_p2p_ie23a(pwdinfo, pframe); + pframe += p2pielen; + pattrib->pktlen += p2pielen; + } + +#ifdef CONFIG_8723AU_P2P + if (pwdinfo->wfd_info->wfd_enable) { + wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } else if (pmlmepriv->wfd_probe_resp_ie && + pmlmepriv->wfd_probe_resp_ie_len > 0) { + /* WFD IE */ + memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, + pmlmepriv->wfd_probe_resp_ie_len); + pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len; + pframe += pmlmepriv->wfd_probe_resp_ie_len; + } +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static int _issue23a_probereq_p2p(struct rtw_adapter *padapter, u8 *da, + int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *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; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&padapter->eeprompriv); + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + if (da) { + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr3, da); + } 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. */ + ether_addr_copy(pwlanhdr->addr1, + pwdinfo->p2p_peer_interface_addr); + ether_addr_copy(pwlanhdr->addr3, + pwdinfo->p2p_peer_interface_addr); + } else { + /* broadcast probe request frame */ + ether_addr_copy(pwlanhdr->addr1, bc_addr); + ether_addr_copy(pwlanhdr->addr3, bc_addr); + } + } + ether_addr_copy(pwlanhdr->addr2, mac); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_PROBEREQ); + + pframe += sizeof (struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) { + pframe = rtw_set_ie23a(pframe, _SSID_IE_, + pwdinfo->tx_prov_disc_info.ssid.ssid_len, + pwdinfo->tx_prov_disc_info.ssid.ssid, + &pattrib->pktlen); + } else { + pframe = rtw_set_ie23a(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_ie23a(pframe, _SUPPORTEDRATES_IE_, 8, + pwdinfo->support_rate, &pattrib->pktlen); + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + if (pmlmepriv->wps_probe_req_ie && + pmlmepriv->p2p_probe_req_ie) { + /* 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; + + /* P2P IE */ + memcpy(pframe, pmlmepriv->p2p_probe_req_ie, + pmlmepriv->p2p_probe_req_ie_len); + pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len; + pframe += pmlmepriv->p2p_probe_req_ie_len; + } + } else { + + /* WPS IE */ + /* Noted by Albert 20110221 */ + /* According to the WPS specification, all the WPS + attribute is presented by Big Endian. */ + + wpsielen = 0; + /* WPS OUI */ + *(u32*) (wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_UUID_E); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010); + wpsielen += 2; + + /* Value: */ + memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), + ETH_ALEN); + wpsielen += 0x10; + + /* Config Method */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(pwdinfo->supported_wps_cm); + wpsielen += 2; + } + + /* Device Name */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + wpsielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008); + wpsielen += 2; + + /* Value: */ + /* Category ID */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI); + wpsielen += 2; + + /* OUI */ + *(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* Sub Category ID */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP); + wpsielen += 2; + + /* Device Password ID */ + /* Type: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + /* Registrar-specified */ + *(u16*) (wpsie + wpsielen) = + cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); + wpsielen += 2; + + pframe = rtw_set_ie23a(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: */ + *(u16*) (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: */ + *(u16*) (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 */ + /* listen channel */ + p2pie[p2pielen++] = pwdinfo->listen_channel; + + /* Extended Listen Timing */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16*) (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: */ + *(u16*) (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 */ + /* operating channel number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; + } + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, + (unsigned char *)p2pie, + &pattrib->pktlen); + + if (pmlmepriv->wps_probe_req_ie) { + /* 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; + } + } + +#ifdef CONFIG_8723AU_P2P + if (pwdinfo->wfd_info->wfd_enable) { + wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } else if (pmlmepriv->wfd_probe_req_ie && + pmlmepriv->wfd_probe_req_ie_len>0) { + /* WFD IE */ + memcpy(pframe, pmlmepriv->wfd_probe_req_ie, + pmlmepriv->wfd_probe_req_ie_len); + pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len; + pframe += pmlmepriv->wfd_probe_req_ie_len; + } +#endif /* CONFIG_8723AU_P2P */ + + 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_mgntframe23a_and_wait_ack23a(padapter, pmgntframe); + } else { + dump_mgntframe23a(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +inline void issue23a_probereq_p2p(struct rtw_adapter *adapter, u8 *da) +{ + _issue23a_probereq_p2p(adapter, da, false); +} + +int issue23a_probereq_p2p_ex(struct rtw_adapter *adapter, u8 *da, + int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + unsigned long start = jiffies; + + do { + ret = _issue23a_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) + msleep(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_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d " + "in %u ms\n", FUNC_ADPT_ARG(adapter), + MAC_ARG(da), rtw_get_oper_ch23a(adapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + else + DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(adapter), + rtw_get_oper_ch23a(adapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + } +exit: + return ret; +} + +#endif /* CONFIG_8723AU_P2P */ + +static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token) +{ + struct rtw_adapter *adapter = recv_frame->adapter; + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct sk_buff *skb = recv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u16 seq_ctrl; + + seq_ctrl = ((recv_frame->attrib.seq_num&0xffff) << 4) | + (recv_frame->attrib.frag_num & 0xf); + + if (ieee80211_has_retry(hdr->frame_control)) { + if (token >= 0) { + if ((seq_ctrl == mlmeext->action_public_rxseq) && + (token == mlmeext->action_public_dialog_token)) { + DBG_8723A(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_8723A(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_public23a_p2p(struct recv_frame *precv_frame) +{ + struct rtw_adapter *padapter = precv_frame->adapter; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint len = skb->len; + u8 *frame_body; + u8 dialogToken = 0; +#ifdef CONFIG_8723AU_P2P + u8 *p2p_ie; + u32 p2p_ielen; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 result = P2P_STATUS_SUCCESS; +#endif /* CONFIG_8723AU_P2P */ + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + dialogToken = frame_body[7]; + + if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL) + return _FAIL; + +#ifdef CONFIG_8723AU_P2P + del_timer_sync(&pwdinfo->reset_ch_sitesurvey); + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len); + } else { + /* 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 ieee80211_hdr_3addr); + + switch (frame_body[ 6 ])/* OUI Subtype */ + { + case P2P_GO_NEGO_REQ: + DBG_8723A("[%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. */ + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + /* Restore the previous p2p state */ + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + DBG_8723A("[%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 (is_zero_ether_addr(pwdinfo->rx_prov_disc_info.peerDevAddr)) + ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2); + + result = process_p2p_group_negotation_req23a(pwdinfo, frame_body, len); + issue_p2p_GO_response(padapter, hdr->addr2, + frame_body, len, result); + + /* Commented by Albert 20110718 */ + /* No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */ + mod_timer(&pwdinfo->restore_p2p_state_timer, + jiffies + msecs_to_jiffies(5000)); + break; + + case P2P_GO_NEGO_RESP: + DBG_8723A("[%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. */ + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + pwdinfo->nego_req_info.benable = false; + result = process_p2p_group_negotation_resp23a(pwdinfo, frame_body, len); + issue_p2p_GO_confirm(pwdinfo->padapter, + hdr->addr2, + result); + if (result == P2P_STATUS_SUCCESS) { + 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; + mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(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)) + { + mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000)); + } + } else { + DBG_8723A("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__); + } + + break; + + case P2P_GO_NEGO_CONF: + + DBG_8723A("[%s] Got GO Nego Confirm Frame\n", __func__); + result = process_p2p_group_negotation_confirm23a(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; + mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH)); + } + } + break; + + case P2P_INVIT_REQ: + /* Added by Albert 2010/10/05 */ + /* Received the P2P Invite Request frame. */ + + DBG_8723A("[%s] Got invite request frame!\n", __func__); + if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen))) + { + /* 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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen); + if (attr_contentlen) + { + + rtw_get_p2p_attr23a_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_8723A("[%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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen); + if (attr_contentlen) { + if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) { + /* 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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + if (rtw_ch_set_search_ch23a(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; + mod_timer(&pwdinfo->reset_ch_sitesurvey, jiffies + msecs_to_jiffies(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; + mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(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_8723A("[%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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen); + if (attr_contentlen) + { + if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) { + /* 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 */ + ether_addr_copy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr); + 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_8723A("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + } + } + else + { + DBG_8723A("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + + DBG_8723A("[%s] status_code = %d\n", __func__, status_code); + + pwdinfo->inviteresp_info.token = frame_body[ 7 ]; + issue_p2p_invitation_response23a(padapter, hdr->addr2, pwdinfo->inviteresp_info.token, status_code); + } + break; + + case P2P_INVIT_RESP: + { + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] Got invite response frame!\n", __func__); + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen))) + { + rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + + if (attr_contentlen == 1) + { + DBG_8723A("[%s] Status = %d\n", __func__, attr_content); + pwdinfo->invitereq_info.benable = false; + + if (attr_content == P2P_STATUS_SUCCESS) + { + if (ether_addr_equal(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv))) { + 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)) { + mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000)); + } + break; + } + case P2P_DEVDISC_REQ: + + process_p2p_devdisc_req23a(pwdinfo, pframe, len); + + break; + + case P2P_DEVDISC_RESP: + + process_p2p_devdisc_resp23a(pwdinfo, pframe, len); + + break; + + case P2P_PROVISION_DISC_REQ: + DBG_8723A("[%s] Got Provisioning Discovery Request Frame\n", __func__); + process_p2p_provdisc_req23a(pwdinfo, pframe, len); + ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2); + + /* 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); + mod_timer(&pwdinfo->restore_p2p_state_timer, + jiffies + msecs_to_jiffies(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_8723A("[%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. */ + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP); + process_p2p_provdisc_resp23a(pwdinfo, pframe); + mod_timer(&pwdinfo->restore_p2p_state_timer, + jiffies + msecs_to_jiffies(P2P_PROVISION_TIMEOUT)); + break; + + } + } +#endif /* CONFIG_8723AU_P2P */ + + return _SUCCESS; +} + +static unsigned int on_action_public23a_vendor(struct recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + struct sk_buff *skb = precv_frame->pkt; + u8 *pframe = skb->data; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + + if (!memcmp(frame_body + 2, P2P_OUI23A, 4)) { + ret = on_action_public23a_p2p(precv_frame); + } + + return ret; +} + +static unsigned int +on_action_public23a_default(struct recv_frame *precv_frame, u8 action) +{ + unsigned int ret = _FAIL; + struct sk_buff *skb = precv_frame->pkt; + u8 *pframe = skb->data; + uint frame_len = skb->len; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + u8 token; + struct rtw_adapter *adapter = precv_frame->adapter; + int cnt = 0; + char msg[64]; + + token = frame_body[2]; + + if (rtw_action_public_decache(precv_frame, token) == _FAIL) + goto exit; + + cnt += sprintf((msg+cnt), "%s(token:%u)", + action_public_str23a(action), token); + rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg); + + ret = _SUCCESS; + +exit: + return ret; +} + +unsigned int on_action_public23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + u8 category, action; + + /* check RA matches or not */ + if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1)) + goto exit; + + category = frame_body[0]; + if (category != WLAN_CATEGORY_PUBLIC) + goto exit; + + action = frame_body[1]; + switch (action) { + case ACT_PUBLIC_VENDOR: + ret = on_action_public23a_vendor(precv_frame); + break; + default: + ret = on_action_public23a_default(precv_frame, action); + break; + } + +exit: + return ret; +} + +unsigned int OnAction23a_ht(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction23a_wmm(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction23a_p2p(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_P2P + u8 *frame_body; + u8 category, OUI_Subtype, dialogToken = 0; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + uint len = skb->len; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + DBG_8723A("%s\n", __func__); + + /* check RA matches or not */ + if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1)) + return _SUCCESS; + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + category = frame_body[0]; + if (category != WLAN_CATEGORY_VENDOR_SPECIFIC) + return _SUCCESS; + + if (cpu_to_be32(*((u32*) (frame_body + 1))) != P2POUI) + return _SUCCESS; + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + rtw_cfg80211_rx_action_p2p(padapter, pframe, len); + return _SUCCESS; + } else { + len -= sizeof(struct ieee80211_hdr_3addr); + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + + switch (OUI_Subtype) + { + case P2P_NOTICE_OF_ABSENCE: + break; + + case P2P_PRESENCE_REQUEST: + process_p2p_presence_req23a(pwdinfo, pframe, len); + break; + + case P2P_PRESENCE_RESPONSE: + break; + + case P2P_GO_DISC_REQUEST: + break; + + default: + break; + } + } +#endif /* CONFIG_8723AU_P2P */ + + return _SUCCESS; +} + +unsigned int OnAction23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + int i; + unsigned char category; + struct action_handler *ptable; + unsigned char *frame_body; + struct sk_buff *skb = precv_frame->pkt; + u8 *pframe = skb->data; + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + category = frame_body[0]; + + for (i = 0; + i < sizeof(OnAction23a_tbl) / sizeof(struct action_handler); i++) { + ptable = &OnAction23a_tbl[i]; + + if (category == ptable->num) + ptable->func(padapter, precv_frame); + } + + return _SUCCESS; +} + +unsigned int DoReserved23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + return _SUCCESS; +} + +struct xmit_frame *alloc_mgtxmitframe23a(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pmgntframe; + struct xmit_buf *pxmitbuf; + + pmgntframe = rtw_alloc_xmitframe23a_ext(pxmitpriv); + + if (!pmgntframe) { + DBG_8723A(FUNC_ADPT_FMT" alloc xmitframe fail\n", + FUNC_ADPT_ARG(pxmitpriv->adapter)); + goto exit; + } + + pxmitbuf = rtw_alloc_xmitbuf23a_ext(pxmitpriv); + if (!pxmitbuf) { + DBG_8723A(FUNC_ADPT_FMT" alloc xmitbuf fail\n", + FUNC_ADPT_ARG(pxmitpriv->adapter)); + rtw_free_xmitframe23a(pxmitpriv, pmgntframe); + pmgntframe = NULL; + goto exit; + } + + pmgntframe->frame_tag = MGNT_FRAMETAG; + pmgntframe->pxmitbuf = pxmitbuf; + pmgntframe->buf_addr = pxmitbuf->pbuf; + pxmitbuf->priv_data = pmgntframe; + +exit: + return pmgntframe; +} + +/**************************************************************************** + +Following are some TX fuctions for WiFi MLME + +*****************************************************************************/ + +void update_mgnt_tx_rate23a(struct rtw_adapter *padapter, u8 rate) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + pmlmeext->tx_rate = rate; + DBG_8723A("%s(): rate = %x\n", __func__, rate); +} + +void update_mgntframe_attrib23a(struct rtw_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_mgntframe23a(struct rtw_adapter *padapter, + struct xmit_frame *pmgntframe) +{ + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) + return; + + rtw_hal_mgnt_xmit23a(padapter, pmgntframe); +} + +s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter, + struct xmit_frame *pmgntframe, int timeout_ms) +{ + s32 ret = _FAIL; + unsigned long irqL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf; + struct submit_ctx sctx; + + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) + return ret; + + rtw_sctx_init23a(&sctx, timeout_ms); + pxmitbuf->sctx = &sctx; + + ret = rtw_hal_mgnt_xmit23a(padapter, pmgntframe); + + if (ret == _SUCCESS) + ret = rtw_sctx_wait23a(&sctx); + + spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL); + pxmitbuf->sctx = NULL; + spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL); + + return ret; +} + +s32 dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter, + struct xmit_frame *pmgntframe) +{ + s32 ret = _FAIL; + u32 timeout_ms = 500;/* 500ms */ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) + return -1; + + mutex_lock(&pxmitpriv->ack_tx_mutex); + pxmitpriv->ack_tx = true; + + pmgntframe->ack_report = 1; + if (rtw_hal_mgnt_xmit23a(padapter, pmgntframe) == _SUCCESS) { + ret = rtw_ack_tx_wait23a(pxmitpriv, timeout_ms); + } + + pxmitpriv->ack_tx = false; + mutex_unlock(&pxmitpriv->ack_tx_mutex); + + 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; + u8 *next_ie; + u32 remain_len; + + ssid_ie = rtw_get_ie23a(ies, WLAN_EID_SSID, &ssid_len_ori, ies_len); + + /* DBG_8723A("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", + __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */ + + if (ssid_ie && ssid_len_ori > 0) { + switch (hidden_ssid_mode) + { + case 1: + next_ie = ssid_ie + 2 + ssid_len_ori; + 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_beacon23a(struct rtw_adapter *padapter, int timeout_ms) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned int rate_len; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; +#ifdef CONFIG_8723AU_AP_MODE + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#endif + 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_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_P2P */ + u8 *wps_ie; + u32 wps_ielen; + u8 sr = 0; + int len_diff; + + /* DBG_8723A("%s\n", __func__); */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) { + DBG_8723A("%s, alloc mgnt frame fail\n", __func__); + return; + } +#ifdef CONFIG_8723AU_AP_MODE + spin_lock_bh(&pmlmepriv->bcn_update_lock); +#endif + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + pattrib->qsel = 0x10; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, bc_addr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(cur_network)); + + SetSeqNum(pwlanhdr, 0 /*pmlmeext->mgnt_seq*/); + /* pmlmeext->mgnt_seq++; */ + SetFrameSubType(pframe, WIFI_BEACON); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) { + /* DBG_8723A("ie len =%d\n", cur_network->IELength); */ +#ifdef CONFIG_8723AU_P2P + /* for P2P : Primary Device Type & Device Name */ + u32 insert_len = 0; + wps_ie = rtw_get_wps_ie23a(cur_network->IEs + _FIXED_IE_LENGTH_, + cur_network->IELength - + _FIXED_IE_LENGTH_, NULL, &wps_ielen); + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wps_ie && + wps_ielen > 0) { + uint wps_offset, remainder_ielen; + u8 *premainder_ie, *pframe_wscie; + + wps_offset = (uint)(wps_ie - cur_network->IEs); + + premainder_ie = wps_ie + wps_ielen; + + remainder_ielen = cur_network->IELength - wps_offset - + wps_ielen; + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + if (pmlmepriv->wps_beacon_ie && + pmlmepriv->wps_beacon_ie_len>0) { + memcpy(pframe, cur_network->IEs, + wps_offset); + pframe += wps_offset; + pattrib->pktlen += wps_offset; + + memcpy(pframe, pmlmepriv->wps_beacon_ie, + pmlmepriv->wps_beacon_ie_len); + pframe += pmlmepriv->wps_beacon_ie_len; + pattrib->pktlen += + pmlmepriv->wps_beacon_ie_len; + + /* copy remainder_ie to pframe */ + 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 { + pframe_wscie = pframe + wps_offset; + memcpy(pframe, cur_network->IEs, + wps_offset + wps_ielen); + pframe += (wps_offset + wps_ielen); + pattrib->pktlen += (wps_offset + wps_ielen); + + /* now pframe is end of wsc ie, insert Primary + Device Type & Device Name */ + /* Primary Device Type */ + /* Type: */ + *(u16*) (pframe + insert_len) = + cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + insert_len += 2; + + /* Length: */ + *(u16*) (pframe + insert_len) = + cpu_to_be16(0x0008); + insert_len += 2; + + /* Value: */ + /* Category ID */ + *(u16*) (pframe + insert_len) = + cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + insert_len += 2; + + /* OUI */ + *(u32*) (pframe + insert_len) = + cpu_to_be32(WPSOUI); + insert_len += 4; + + /* Sub Category ID */ + *(u16*) (pframe + insert_len) = + cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + insert_len += 2; + + /* Device Name */ + /* Type: */ + *(u16*) (pframe + insert_len) = + cpu_to_be16(WPS_ATTR_DEVICE_NAME); + insert_len += 2; + + /* Length: */ + *(u16*) (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) = (wps_ielen -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_8723AU_P2P */ + 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); + + wps_ie = rtw_get_wps_ie23a(pmgntframe->buf_addr + TXDESC_OFFSET+ + sizeof (struct ieee80211_hdr_3addr) + + _BEACON_IE_OFFSET_, pattrib->pktlen - + sizeof (struct ieee80211_hdr_3addr) - + _BEACON_IE_OFFSET_, NULL, + &wps_ielen); + if (wps_ie && wps_ielen > 0) { + rtw_get_wps_attr_content23a(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_8723AU_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + u32 len; + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + len = pmlmepriv->p2p_beacon_ie_len; + if (pmlmepriv->p2p_beacon_ie && len > 0) + memcpy(pframe, + pmlmepriv->p2p_beacon_ie, len); + } else + len = build_beacon_p2p_ie23a(pwdinfo, pframe); + + pframe += len; + pattrib->pktlen += len; + + if (true == pwdinfo->wfd_info->wfd_enable) { + len = build_beacon_wfd_ie(pwdinfo, pframe); + } else { + len = 0; + if (pmlmepriv->wfd_beacon_ie && + pmlmepriv->wfd_beacon_ie_len>0) { + len = pmlmepriv->wfd_beacon_ie_len; + memcpy(pframe, + pmlmepriv->wfd_beacon_ie, len); + } + } + pframe += len; + pattrib->pktlen += len; + } +#endif /* CONFIG_8723AU_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_interval23a_from_ie(cur_network->IEs), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + memcpy(pframe, (unsigned char *) + rtw_get_capability23a_from_ie(cur_network->IEs), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* SSID */ + pframe = rtw_set_ie23a(pframe, _SSID_IE_, cur_network->Ssid.ssid_len, + cur_network->Ssid.ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates); + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, + ((rate_len > 8)? 8: rate_len), + cur_network->SupportedRates, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie23a(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_ie23a(pframe, _IBSS_PARA_IE_, 2, + (unsigned char *)&ATIMWindow, + &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1, + &erpinfo, &pattrib->pktlen); + } + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) + pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, + rate_len - 8, + cur_network->SupportedRates + 8, + &pattrib->pktlen); + + /* todo:HT for adhoc */ + +_issue_bcn: + +#ifdef CONFIG_8723AU_AP_MODE + pmlmepriv->update_bcn = false; + + spin_unlock_bh(&pmlmepriv->bcn_update_lock); +#endif + + if ((pattrib->pktlen + TXDESC_SIZE) > 512) { + DBG_8723A("beacon frame too large\n"); + return; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + /* DBG_8723A("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */ + if (timeout_ms > 0) + dump_mgntframe23a_and_wait(padapter, pmgntframe, timeout_ms); + else + dump_mgntframe23a(padapter, pmgntframe); +} + +void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da, + u8 is_valid_p2p_probereq) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac, *bssid; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; +#ifdef CONFIG_8723AU_AP_MODE + u8 *pwps_ie; + uint wps_ielen; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#endif + 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_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_P2P */ + u8 *ssid_ie; + int ssid_ielen; + int ssid_ielen_diff; + u8 buf[MAX_IE_SZ]; + u8 *ies; + + /* DBG_8723A("%s\n", __func__); */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + DBG_8723A("%s, alloc mgnt frame fail\n", __func__); + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&padapter->eeprompriv); + bssid = cur_network->MacAddress; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr2, mac); + ether_addr_copy(pwlanhdr->addr3, bssid); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(fctrl, WIFI_PROBERSP); + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = pattrib->hdrlen; + pframe += pattrib->hdrlen; + + if (cur_network->IELength > MAX_IE_SZ) + return; + +#ifdef CONFIG_8723AU_AP_MODE + if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) { + pwps_ie = rtw_get_wps_ie23a(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; + + /* to get ie data len */ + wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1]; + 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; + } + + /* retrieve SSID IE from cur_network->Ssid */ + ies = pmgntframe->buf_addr + TXDESC_OFFSET + + sizeof(struct ieee80211_hdr_3addr); + + ssid_ie = rtw_get_ie23a(ies+_FIXED_IE_LENGTH_, _SSID_IE_, + &ssid_ielen, + (pframe-ies)-_FIXED_IE_LENGTH_); + + ssid_ielen_diff = cur_network->Ssid.ssid_len - ssid_ielen; + + if (ssid_ie && cur_network->Ssid.ssid_len) { + uint remainder_ielen; + u8 *remainder_ie; + remainder_ie = ssid_ie + 2; + remainder_ielen = (pframe-remainder_ie); + + DBG_8723A_LEVEL(_drv_warning_, FUNC_ADPT_FMT + " remainder_ielen > MAX_IE_SZ\n", + FUNC_ADPT_ARG(padapter)); + if (remainder_ielen > MAX_IE_SZ) { + remainder_ielen = MAX_IE_SZ; + } + + memcpy(buf, remainder_ie, remainder_ielen); + memcpy(remainder_ie+ssid_ielen_diff, buf, + remainder_ielen); + *(ssid_ie+1) = cur_network->Ssid.ssid_len; + memcpy(ssid_ie+2, cur_network->Ssid.ssid, + cur_network->Ssid.ssid_len); + + pframe += ssid_ielen_diff; + pattrib->pktlen += ssid_ielen_diff; + } + } else +#endif + { + + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + + memcpy(pframe, (unsigned char *) + rtw_get_beacon_interval23a_from_ie(cur_network->IEs), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + memcpy(pframe, (unsigned char *) + rtw_get_capability23a_from_ie(cur_network->IEs), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* below for ad-hoc mode */ + + /* SSID */ + pframe = rtw_set_ie23a(pframe, _SSID_IE_, + cur_network->Ssid.ssid_len, + cur_network->Ssid.ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates); + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, + ((rate_len > 8)? 8: rate_len), + cur_network->SupportedRates, + &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie23a(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_ie23a(pframe, _IBSS_PARA_IE_, 2, + (unsigned char *)&ATIMWindow, + &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1, + &erpinfo, &pattrib->pktlen); + } + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) + pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, + rate_len - 8, + cur_network->SupportedRates + 8, + &pattrib->pktlen); + + /* todo:HT for adhoc */ + } + +#ifdef CONFIG_8723AU_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) { + u32 len; + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + /* if pwdinfo->role == P2P_ROLE_DEVICE will call + issue_probersp23a_p2p23a() */ + len = pmlmepriv->p2p_go_probe_resp_ie_len; + if (pmlmepriv->p2p_go_probe_resp_ie && len>0) + memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, + len); + } else + len = build_probe_resp_p2p_ie23a(pwdinfo, pframe); + + pframe += len; + pattrib->pktlen += len; + + if (true == pwdinfo->wfd_info->wfd_enable) { + len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0); + } else { + len = 0; + if (pmlmepriv->wfd_probe_resp_ie && + pmlmepriv->wfd_probe_resp_ie_len > 0) { + len = pmlmepriv->wfd_probe_resp_ie_len; + memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, + len); + } + } + pframe += len; + pattrib->pktlen += len; + } +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static int _issue_probereq23a(struct rtw_adapter *padapter, + struct cfg80211_ssid *pssid, u8 *da, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *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_probereq23a\n")); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&padapter->eeprompriv); + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + if (da) { + /* unicast probe request frame */ + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr3, da); + } else { + /* broadcast probe request frame */ + ether_addr_copy(pwlanhdr->addr1, bc_addr); + ether_addr_copy(pwlanhdr->addr3, bc_addr); + } + + ether_addr_copy(pwlanhdr->addr2, mac); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_PROBEREQ); + + pframe += sizeof (struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr); + + if (pssid) + pframe = rtw_set_ie23a(pframe, _SSID_IE_, pssid->ssid_len, + pssid->ssid, &pattrib->pktlen); + else + pframe = rtw_set_ie23a(pframe, _SSID_IE_, 0, NULL, + &pattrib->pktlen); + + get_rate_set23a(padapter, bssrate, &bssrate_len); + + if (bssrate_len > 8) { + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8, + bssrate, &pattrib->pktlen); + pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, + (bssrate_len - 8), (bssrate + 8), + &pattrib->pktlen); + } else { + pframe = rtw_set_ie23a(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_mgntframe23a_and_wait_ack23a(padapter, pmgntframe); + } else { + dump_mgntframe23a(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +inline void issue_probereq23a(struct rtw_adapter *padapter, + struct cfg80211_ssid *pssid, u8 *da) +{ + _issue_probereq23a(padapter, pssid, da, false); +} + +int issue_probereq23a_ex23a(struct rtw_adapter *padapter, + struct cfg80211_ssid *pssid, u8 *da, + int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + unsigned long start = jiffies; + + do { + ret = _issue_probereq23a(padapter, pssid, da, + wait_ms > 0 ? true : false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(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_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d " + "in %u ms\n", FUNC_ADPT_ARG(padapter), + MAC_ARG(da), rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + else + DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), + rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + } +exit: + return ret; +} + +/* if psta == NULL, indiate we are station(client) now... */ +void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta, + unsigned short status) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned int val32; + unsigned short val16; + 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; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_AUTH); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + if (psta) { /* for AP mode */ +#ifdef CONFIG_8723AU_AP_MODE + + ether_addr_copy(pwlanhdr->addr1, psta->hwaddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv)); + + /* setting auth algo number */ + val16 = (u16)psta->authalg; + + if (status != WLAN_STATUS_SUCCESS) + val16 = 0; + + if (val16) { + val16 = cpu_to_le16(val16); + use_shared_key = 1; + } + + pframe = rtw_set_fixed_ie23a(pframe, _AUTH_ALGM_NUM_, + (unsigned char *)&val16, + &pattrib->pktlen); + + /* setting auth seq number */ + val16 = (u16)psta->auth_seq; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie23a(pframe, _AUTH_SEQ_NUM_, + (unsigned char *)&val16, + &pattrib->pktlen); + + /* setting status code... */ + val16 = status; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie23a(pframe, _STATUS_CODE_, + (unsigned char *)&val16, + &pattrib->pktlen); + + /* added challenging text... */ + if ((psta->auth_seq == 2) && + (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) + pframe = rtw_set_ie23a(pframe, _CHLGETXT_IE_, 128, + psta->chg_txt, &pattrib->pktlen); +#endif + } else { + ether_addr_copy(pwlanhdr->addr1, + get_my_bssid23a(&pmlmeinfo->network)); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, + get_my_bssid23a(&pmlmeinfo->network)); + + /* setting auth algo number */ + /* 0:OPEN System, 1:Shared key */ + val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0; + if (val16) { + val16 = cpu_to_le16(val16); + use_shared_key = 1; + } + /* DBG_8723A("%s auth_algo = %s auth_seq =%d\n", __func__, + (pmlmeinfo->auth_algo == 0)?"OPEN":"SHARED", + pmlmeinfo->auth_seq); */ + + /* setting IV for auth seq #3 */ + if ((pmlmeinfo->auth_seq == 3) && + (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && + (use_shared_key == 1)) { + /* DBG_8723A("==> iv(%d), key_index(%d)\n", + pmlmeinfo->iv, pmlmeinfo->key_index); */ + val32 = ((pmlmeinfo->iv++) | + (pmlmeinfo->key_index << 30)); + val32 = cpu_to_le32(val32); + pframe = rtw_set_fixed_ie23a(pframe, 4, + (unsigned char *)&val32, + &pattrib->pktlen); + + pattrib->iv_len = 4; + } + + pframe = rtw_set_fixed_ie23a(pframe, _AUTH_ALGM_NUM_, + (unsigned char *)&val16, + &pattrib->pktlen); + + /* setting auth seq number */ + val16 = pmlmeinfo->auth_seq; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie23a(pframe, _AUTH_SEQ_NUM_, + (unsigned char *)&val16, + &pattrib->pktlen); + + /* setting status code... */ + val16 = status; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie23a(pframe, _STATUS_CODE_, + (unsigned char *)&val16, + &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_ie23a(pframe, _CHLGETXT_IE_, 128, + pmlmeinfo->chg_txt, + &pattrib->pktlen); + + SetPrivacy(fctrl); + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + + pattrib->encrypt = _WEP40_; + + pattrib->icv_len = 4; + + pattrib->pktlen += pattrib->icv_len; + } + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + rtw_wep_encrypt23a(padapter, pmgntframe); + DBG_8723A("%s\n", __func__); + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status, + struct sta_info *pstat, int pkt_type) +{ +#ifdef CONFIG_8723AU_AP_MODE + struct xmit_frame *pmgntframe; + struct ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + unsigned char *pbuf, *pframe; + unsigned short val; + 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; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + DBG_8723A("%s\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + pwlanhdr->frame_control = 0; + + ether_addr_copy(pwlanhdr->addr1, pstat->hwaddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + 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 ieee80211_hdr_3addr); + pattrib->pktlen += pattrib->hdrlen; + pframe += pattrib->hdrlen; + + /* capability */ + val = *(unsigned short *)rtw_get_capability23a_from_ie(ie); + + pframe = rtw_set_fixed_ie23a(pframe, _CAPABILITY_, + (unsigned char *)&val, &pattrib->pktlen); + + status = cpu_to_le16(status); + pframe = rtw_set_fixed_ie23a(pframe, _STATUS_CODE_, + (unsigned char *)&status, + &pattrib->pktlen); + + val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15)); + pframe = rtw_set_fixed_ie23a(pframe, _ASOC_ID_, (unsigned char *)&val, + &pattrib->pktlen); + + if (pstat->bssratelen <= 8) { + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, + pstat->bssratelen, pstat->bssrateset, + &pattrib->pktlen); + } else { + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8, + pstat->bssrateset, &pattrib->pktlen); + pframe = rtw_set_ie23a(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 */ + /* p = hostapd_eid_ht_capabilities_info(hapd, p); */ + pbuf = rtw_get_ie23a(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 */ + /* p = hostapd_eid_ht_operation(hapd, p); */ + pbuf = rtw_get_ie23a(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_ie23a(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) || (ie_len == 0)) + break; + } + } + + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) { + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6, + REALTEK_96B_IE23A, &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_8723AU_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && + pwdinfo->wfd_info->wfd_enable) { + wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +#endif +} + +void issue_assocreq23a(struct rtw_adapter *padapter) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe, *p; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned short val16; + unsigned int i, j, ie_len, index = 0; + unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates]; + struct ndis_802_11_var_ies *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_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 p2pie[255] = { 0x00 }; + u16 p2pielen = 0; + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network)); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ASSOCREQ); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* caps */ + memcpy(pframe, rtw_get_capability23a_from_ie(pmlmeinfo->network.IEs), + 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* listen interval */ + /* todo: listen interval for power saving */ + val16 = cpu_to_le16(3); + memcpy(pframe, (unsigned char *)&val16, 2); + pframe += 2; + pattrib->pktlen += 2; + + /* SSID */ + pframe = rtw_set_ie23a(pframe, _SSID_IE_, + pmlmeinfo->network.Ssid.ssid_len, + pmlmeinfo->network.Ssid.ssid, &pattrib->pktlen); + + /* supported rate & extended supported rate */ + + get_rate_set23a(padapter, sta_bssrate, &sta_bssrate_len); + /* DBG_8723A("sta_bssrate_len =%d\n", sta_bssrate_len); */ + + /* for JAPAN, channel 14 can only uses B Mode(CCK) */ + if (pmlmeext->cur_channel == 14) + sta_bssrate_len = 4; + + /* for (i = 0; i < sta_bssrate_len; i++) { */ + /* DBG_8723A("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */ + /* */ + + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (pmlmeinfo->network.SupportedRates[i] == 0) + break; + DBG_8723A("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)) { + /* DBG_8723A("match i = %d, j =%d\n", i, j); */ + break; + } + } + + if (j == sta_bssrate_len) { + /* the rate is not supported by STA */ + DBG_8723A("%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_8723A("bssrate_len = %d\n", bssrate_len); + + if (bssrate_len == 0) { + rtw_free_xmitbuf23a(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe23a(pxmitpriv, pmgntframe); + goto exit; /* don't connect to AP if no joint supported rate */ + } + + if (bssrate_len > 8) { + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8, + bssrate, &pattrib->pktlen); + pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, + (bssrate_len - 8), (bssrate + 8), + &pattrib->pktlen); + } else + pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, + bssrate_len, bssrate, &pattrib->pktlen); + + /* RSN */ + p = rtw_get_ie23a((pmlmeinfo->network.IEs + + sizeof(struct ndis_802_11_fixed_ies)), _RSN_IE_2_, + &ie_len, (pmlmeinfo->network.IELength - + sizeof(struct ndis_802_11_fixed_ies))); + if (p) + pframe = rtw_set_ie23a(pframe, _RSN_IE_2_, ie_len, (p + 2), + &pattrib->pktlen); + + /* HT caps */ + if (padapter->mlmepriv.htpriv.ht_option == true) { + p = rtw_get_ie23a((pmlmeinfo->network.IEs + + sizeof(struct ndis_802_11_fixed_ies)), + _HT_CAPABILITY_IE_, &ie_len, + (pmlmeinfo->network.IELength - + sizeof(struct ndis_802_11_fixed_ies))); + if ((p != NULL) && (!(is_ap_in_tkip23a(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 &= (~(BIT(6) | BIT(1))); + } else { + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= BIT(1); + } + + /* todo: disable SM power save mode */ + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= + 0x000c; + + rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, + (u8 *)(&rf_type)); + /* switch (pregpriv->rf_config) */ + 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_1R23A, 16); + break; + + case RF_2T2R: + case RF_1T2R: + default: + + /* enable for 2.4/5 GHz */ + if ((pregpriv->rx_stbc == 0x3) || + ((pmlmeext->cur_wireless_mode & + WIRELESS_11_24N) && + /* enable for 2.4GHz */ + (pregpriv->rx_stbc == 0x1)) || + ((pmlmeext->cur_wireless_mode & + WIRELESS_11_5N) && + (pregpriv->rx_stbc == 0x2)) || + /* enable for 5GHz */ + (pregpriv->wifi_spec == 1)) { + DBG_8723A("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_2R23A, 16); + break; + } + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = + cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info); + +#ifdef CONFIG_8723AU_BT_COEXIST + if (BT_1Ant(padapter) == true) { + /* set to 8K */ + pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para &= (u8)~IEEE80211_HT_AMPDU_PARM_FACTOR; +/* pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para |= MAX_AMPDU_FACTOR_8K */ + } +#endif + + pframe = rtw_set_ie23a(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_ies); + i < pmlmeinfo->network.IELength;) { + pIE = (struct ndis_802_11_var_ies *) + (pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) + { + case _VENDOR_SPECIFIC_IE_: + if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) || + !memcmp(pIE->data, WMM_OUI23A, 4) || + !memcmp(pIE->data, WPS_OUI23A, 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_OUI23A, 4)) + pIE->Length = 14; + } + pframe = rtw_set_ie23a(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_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6, + REALTEK_96B_IE23A, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + + if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) { + if (pmlmepriv->p2p_assoc_req_ie && + pmlmepriv->p2p_assoc_req_ie_len>0) { + memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, + pmlmepriv->p2p_assoc_req_ie_len); + pframe += pmlmepriv->p2p_assoc_req_ie_len; + pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len; + } + } else { + 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: */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16*) (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) */ + *(u16*) (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)) + *(u16*) (p2pie + p2pielen) = + cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); + else + *(u16*) (p2pie + p2pielen) = + cpu_to_be16(WPS_CONFIG_METHOD_PBC); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16*) (p2pie + p2pielen) = + cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16*) (p2pie + p2pielen) = + cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + /* No Secondary Device Type List */ + p2pie[p2pielen++] = 0x00; + + /* Device Name */ + /* Type: */ + *(u16*) (p2pie + p2pielen) = + cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16*) (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: */ + *(u16*) (p2pie + p2pielen) = cpu_to_le16(0x000D); + p2pielen += 2; + + /* Value: */ + memcpy(p2pie + p2pielen, pwdinfo->device_addr, + ETH_ALEN); /* P2P Device Address */ + p2pielen += ETH_ALEN; + + /* P2P Interface Address Count */ + p2pie[p2pielen++] = 1; + + memcpy(p2pie + p2pielen, pwdinfo->device_addr, + ETH_ALEN); /* P2P Interface Address List */ + p2pielen += ETH_ALEN; + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, + p2pielen, (unsigned char *)p2pie, + &pattrib->pktlen); + + /* wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);*/ + /* pframe += wfdielen; */ + /* pattrib->pktlen += wfdielen; */ + } + } + + if (true == pwdinfo->wfd_info->wfd_enable) { + wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } else if (pmlmepriv->wfd_assoc_req_ie != NULL && + pmlmepriv->wfd_assoc_req_ie_len > 0) { + /* WFD IE */ + memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, + pmlmepriv->wfd_assoc_req_ie_len); + pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len; + pframe += pmlmepriv->wfd_assoc_req_ie_len; + } +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe23a(padapter, pmgntframe); + + ret = _SUCCESS; + +exit: + pmlmepriv->assoc_req_len = 0; + if (ret == _SUCCESS) { + kfree(pmlmepriv->assoc_req); + pmlmepriv->assoc_req = kmalloc(pattrib->pktlen, GFP_ATOMIC); + if (pmlmepriv->assoc_req) { + memcpy(pmlmepriv->assoc_req, pwlanhdr, + pattrib->pktlen); + pmlmepriv->assoc_req_len = pattrib->pktlen; + } + } else + kfree(pmlmepriv->assoc_req); + + return; +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_nulldata23a(struct rtw_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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + + /* DBG_8723A("%s:%d\n", __func__, power_mode); */ + + if (!padapter) + goto exit; + + pxmitpriv = &padapter->xmitpriv; + pmlmeext = &padapter->mlmeextpriv; + pmlmeinfo = &pmlmeext->mlmext_info; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + pattrib->retry_ctrl = false; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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); + + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_DATA_NULL); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe); + else { + dump_mgntframe23a(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_nulldata23a(struct rtw_adapter *padapter, unsigned char *da, + unsigned int power_mode, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + unsigned long 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_bssid23a(&pmlmeinfo->network); + + do { + ret = _issue_nulldata23a(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) + msleep(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_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d " + "in %u ms\n", FUNC_ADPT_ARG(padapter), + MAC_ARG(da), rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + else + DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), + rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + } +exit: + return ret; +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_qos_nulldata23a(struct rtw_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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl, *qc; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + DBG_8723A("%s\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(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 ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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); + + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_QOS_DATA_NULL); + + pframe += sizeof(struct ieee80211_qos_hdr); + pattrib->pktlen = sizeof(struct ieee80211_qos_hdr); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe); + else { + dump_mgntframe23a(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_nulldata23a(struct rtw_adapter *padapter, unsigned char *da, + u16 tid, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + unsigned long 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_bssid23a(&pmlmeinfo->network); + + do { + ret = _issue_qos_nulldata23a(padapter, da, tid, + wait_ms > 0 ? true : false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(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_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d " + "in %u ms\n", FUNC_ADPT_ARG(padapter), + MAC_ARG(da), rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + else + DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), + rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + } +exit: + return ret; +} + +static int _issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da, + unsigned short reason, u8 wait_ack) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *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; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_P2P */ + + /* DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */ + +#ifdef CONFIG_8723AU_P2P + if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && + (pwdinfo->rx_invitereq_info.scan_op_ch_only)) { + mod_timer(&pwdinfo->reset_ch_sitesurvey, + jiffies + msecs_to_jiffies(10)); + } +#endif /* CONFIG_8723AU_P2P */ + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + pattrib->retry_ctrl = false; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, da); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_DEAUTH); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + reason = cpu_to_le16(reason); + pframe = rtw_set_fixed_ie23a(pframe, WLAN_REASON_PREV_AUTH_NOT_VALID, + (unsigned char *)&reason, + &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe); + else { + dump_mgntframe23a(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da, + unsigned short reason) +{ + DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); + return _issue_deauth23a(padapter, da, reason, false); +} + +int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da, + unsigned short reason, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + unsigned long start = jiffies; + + do { + ret = _issue_deauth23a(padapter, da, reason, + wait_ms >0 ? true : false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(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_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d " + "in %u ms\n", FUNC_ADPT_ARG(padapter), + MAC_ARG(da), rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + else + DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), + rtw_get_oper_ch23a(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, + jiffies_to_msecs(jiffies - start)); + } +exit: + return ret; +} + +void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter, + u8 *ra, u8 new_ch, u8 ch_offset) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + u8 category, action; + + DBG_8723A(FUNC_NDEV_FMT" ra ="MAC_FMT", ch:%u, offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), + new_ch, ch_offset); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, ra); /* RA */ + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); /* TA */ + ether_addr_copy(pwlanhdr->addr3, ra); /* DA = RA */ + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* category, action */ + category = WLAN_CATEGORY_SPECTRUM_MGMT; + action = WLAN_ACTION_SPCT_CHL_SWITCH; + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + + pframe = rtw_set_ie23a_ch_switch (pframe, &pattrib->pktlen, 0, + new_ch, 0); + pframe = rtw_set_ie23a_secondary_ch_offset(pframe, &pattrib->pktlen, + hal_ch_offset_to_secondary_ch_offset23a(ch_offset)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr, + unsigned char action, unsigned short status) +{ + u8 category = WLAN_CATEGORY_BACK; + u16 start_seq; + u16 BA_para_set; + u16 reason_code; + u16 BA_timeout_value; + u16 BA_starting_seqctrl; + int max_rx_ampdu_factor; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 *pframe; + struct ieee80211_hdr *pwlanhdr; + u16 *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; +#ifdef CONFIG_8723AU_BT_COEXIST + u8 tendaAPMac[] = {0xC8, 0x3A, 0x35}; +#endif + + DBG_8723A("%s, category =%d, action =%d, status =%d\n", + __func__, category, action, status); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + /* memcpy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN); */ + ether_addr_copy(pwlanhdr->addr1, raddr); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + + status = cpu_to_le16(status); + + if (category != 3) + goto out; + + switch (action) + { + case 0: /* ADDBA req */ + do { + pmlmeinfo->dialogToken++; + } while (pmlmeinfo->dialogToken == 0); + pframe = rtw_set_fixed_ie23a(pframe, 1, &pmlmeinfo->dialogToken, + &pattrib->pktlen); + +#ifdef CONFIG_8723AU_BT_COEXIST + if ((BT_1Ant(padapter) == true) && + ((pmlmeinfo->assoc_AP_vendor != broadcomAP) || + memcmp(raddr, tendaAPMac, 3))) { + /* A-MSDU NOT Supported */ + BA_para_set = 0; + /* immediate Block Ack */ + BA_para_set |= (1 << 1) & + IEEE80211_ADDBA_PARAM_POLICY_MASK; + /* TID */ + BA_para_set |= (status << 2) & + IEEE80211_ADDBA_PARAM_TID_MASK; + /* max buffer size is 8 MSDU */ + BA_para_set |= (8 << 6) & + IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + } else +#endif + { + /* immediate ack & 64 buffer size */ + BA_para_set = (0x1002 | ((status & 0xf) << 2)); + } + BA_para_set = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie23a(pframe, 2, + (unsigned char *)&BA_para_set, + &pattrib->pktlen); + + BA_timeout_value = 5000;/* 5ms */ + BA_timeout_value = cpu_to_le16(BA_timeout_value); + pframe = rtw_set_fixed_ie23a(pframe, 2, (unsigned char *) + &BA_timeout_value, + &pattrib->pktlen); + + /* if ((psta = rtw_get_stainfo23a(pstapriv, + pmlmeinfo->network.MacAddress)) != NULL) */ + if ((psta = rtw_get_stainfo23a(pstapriv, raddr))) { + start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1; + + DBG_8723A("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; + } + + BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl); + pframe = rtw_set_fixed_ie23a(pframe, 2, (unsigned char *)&BA_starting_seqctrl, &pattrib->pktlen); + break; + + case 1: /* ADDBA rsp */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &pmlmeinfo->ADDBA_req.dialog_token, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 2, + (unsigned char *)&status, + &pattrib->pktlen); + rtw_hal_get_def_var23a(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, + &max_rx_ampdu_factor); + if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */ + else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */ + else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_16K) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */ + else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_8K) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); /* 8 buffer size */ + else + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */ + +#ifdef CONFIG_8723AU_BT_COEXIST + if ((BT_1Ant(padapter) == true) && + ((pmlmeinfo->assoc_AP_vendor != broadcomAP) || + memcmp(raddr, tendaAPMac, 3))) { + /* max buffer size is 8 MSDU */ + BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + BA_para_set |= (8 << 6) & + IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + } +#endif + + if (pregpriv->ampdu_amsdu == 0)/* disabled */ + BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0)); + else if (pregpriv->ampdu_amsdu == 1)/* enabled */ + BA_para_set = cpu_to_le16(BA_para_set | BIT(0)); + else /* auto */ + BA_para_set = cpu_to_le16(BA_para_set); + + pframe = rtw_set_fixed_ie23a(pframe, 2, + (unsigned char *)&BA_para_set, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 2, (unsigned char *)&pmlmeinfo->ADDBA_req.BA_timeout_value, &pattrib->pktlen); + break; + case 2:/* DELBA */ + BA_para_set = (status & 0x1F) << 3; + BA_para_set = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie23a(pframe, 2, + (unsigned char *)&BA_para_set, + &pattrib->pktlen); + + reason_code = 37;/* Requested from peer STA as it does not + want to use the mechanism */ + reason_code = cpu_to_le16(reason_code); + pframe = rtw_set_fixed_ie23a(pframe, 2, + (unsigned char *)&reason_code, + &pattrib->pktlen); + break; + default: + break; + } + +out: + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter) +{ + struct list_head *plist, *phead, *ptmp; + unsigned char category, action; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *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 rtw_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 (true == pmlmeinfo->bwmode_updated) + return; + + DBG_8723A("%s\n", __func__); + + category = WLAN_CATEGORY_PUBLIC; + action = ACT_PUBLIC_BSSCOEXIST; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network)); + ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); + ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + + /* */ + if (pmlmepriv->num_FortyMHzIntolerant>0) + { + u8 iedata = 0; + + iedata |= BIT(2);/* 20 MHz BSS Width Request */ + + pframe = rtw_set_ie23a(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; + + list_for_each_safe(plist, ptmp, phead) { + int len; + u8 *p; + struct wlan_bssid_ex *pbss_network; + + pnetwork = container_of(plist, struct wlan_network, + list); + + pbss_network = &pnetwork->network; + + p = rtw_get_ie23a(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_ie23a(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &pattrib->pktlen); + + } + + } + + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +unsigned int send_delba23a(struct rtw_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_stainfo23a(pstapriv, addr); + if (psta == NULL) + return _SUCCESS; + + if (initiator == 0) { /* recipient */ + for (tid = 0; tid < MAXTID; tid++) { + if (psta->recvreorder_ctrl[tid].enable == true) { + DBG_8723A("rx agg disable tid(%d)\n", tid); + issue_action_BA23a(padapter, addr, 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_8723A("tx agg disable tid(%d)\n", tid); + issue_action_BA23a(padapter, addr, 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_beacon23a(struct rtw_adapter *padapter) +{ + u8 bxmitok = false; + int issue = 0; + int poll = 0; + unsigned long start = jiffies; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_BCN_VALID, NULL); + do { + issue_beacon23a(padapter, 100); + issue++; + do { + yield(); + rtw23a_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok)); + poll++; + } while ((poll%10)!= 0 && false == bxmitok && + !padapter->bSurpriseRemoved && + !padapter->bDriverStopped); + + } while (!bxmitok && issue<100 && !padapter->bSurpriseRemoved && + !padapter->bDriverStopped); + + if (padapter->bSurpriseRemoved || padapter->bDriverStopped) + return _FAIL; + + if (!bxmitok) { + DBG_8723A("%s fail! %u ms\n", __func__, rtw_get_passing_time_ms23a(start)); + return _FAIL; + } else { + unsigned int passing_time = jiffies_to_msecs(jiffies - start); + + if (passing_time > 100 || issue > 3) + DBG_8723A("%s success, issue:%d, poll:%d, %u ms\n", + __func__, issue, poll, passing_time); + return _SUCCESS; + } +} + +/**************************************************************************** + +Following are some utitity fuctions for WiFi MLME + +*****************************************************************************/ + +bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel) +{ + + int i = 0; + u8 Channel_5G[45] = {36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, + 60, 62, 64, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, + 124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153, 155, 157, 159, + 161, 163, 165}; + for (i = 0; i < sizeof(Channel_5G); i++) + if (channel == Channel_5G[i]) + return true; + return false; +} + +void site_survey23a(struct rtw_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_8723AU_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)) { + /* 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_ch23a(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_8723AU_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 & IEEE80211_CHAN_NO_IR) ? SCAN_PASSIVE : SCAN_ACTIVE; +} + } + + if (survey_channel != 0) { + /* PAUSE 4-AC Queue when site_survey23a */ + /* rtw23a_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + /* val8 |= 0x0f; */ + /* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + if (pmlmeext->sitesurvey_res.channel_idx == 0) + set_channel_bwmode23a(padapter, survey_channel, + HAL_PRIME_CHNL_OFFSET_DONT_CARE, + HT_CHANNEL_WIDTH_20); + else + SelectChannel23a(padapter, survey_channel); + + if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */ + { +#ifdef CONFIG_8723AU_P2P + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || + rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) + ) + { + issue23a_probereq_p2p(padapter, NULL); + issue23a_probereq_p2p(padapter, NULL); + issue23a_probereq_p2p(padapter, NULL); + } + else +#endif /* CONFIG_8723AU_P2P */ + { + int i; + for (i = 0;isitesurvey_res.ssid[i].ssid_len) { + /* todo: to issue two probe req??? */ + issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL); + /* msleep(SURVEY_TO>>1); */ + issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL); + } + } + + if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) { + /* todo: to issue two probe req??? */ + issue_probereq23a(padapter, NULL, NULL); + /* msleep(SURVEY_TO>>1); */ + issue_probereq23a(padapter, NULL, NULL); + } + } + } + + set_survey_timer(pmlmeext, pmlmeext->chan_scan_time); + } else { + + /* channel number is 0 or this channel is not valid. */ + + +#ifdef CONFIG_8723AU_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_bwmode23a(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_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); + /* turn on dynamic functions */ + Restore_DM_Func_Flag23a(padapter); + /* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */ + + mod_timer(&pwdinfo->find_phase_timer, jiffies + + msecs_to_jiffies(pwdinfo->listen_dwell * 100)); + } else +#endif /* CONFIG_8723AU_P2P */ + { +#ifdef CONFIG_8723AU_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_8723AU_P2P */ + + pmlmeext->sitesurvey_res.state = SCAN_COMPLETE; + + /* switch back to the original channel */ + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + /* flush 4-AC Queue after site_survey23a */ + /* val8 = 0; */ + /* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + + /* config MSR */ + Set_MSR23a(padapter, (pmlmeinfo->state & 0x3)); + + initialgain = 0xff; /* restore RX GAIN */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); + /* turn on dynamic functions */ + Restore_DM_Func_Flag23a(padapter); + /* Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */ + + if (is_client_associated_to_ap23a(padapter) == true) + { + issue_nulldata23a(padapter, NULL, 0, 3, 500); + + } + + val8 = 0; /* survey done */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + report_surveydone_event23a(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_info23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid) +{ + int i; + u32 len; + u8 *p; + u16 val16; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + u32 packet_len = skb->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; + + len = packet_len - sizeof(struct ieee80211_hdr_3addr); + + if (len > MAX_IE_SZ) + { + /* DBG_8723A("IE too long for survey event\n"); */ + return _FAIL; + } + + memset(bssid, 0, sizeof(struct wlan_bssid_ex)); + + if (ieee80211_is_beacon(hdr->frame_control)) { + bssid->reserved = 1; + ie_offset = _BEACON_IE_OFFSET_; + } else { + /* FIXME : more type */ + if (ieee80211_is_probe_req(hdr->frame_control)) { + ie_offset = _PROBEREQ_IE_OFFSET_; + bssid->reserved = 2; + } else if (ieee80211_is_probe_resp(hdr->frame_control)) { + ie_offset = _PROBERSP_IE_OFFSET_; + bssid->reserved = 3; + } else { + bssid->reserved = 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 ieee80211_hdr_3addr)), bssid->IELength); + + /* get the signal strength */ + bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower; /* 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 */ + + /* checking SSID */ + if ((p = rtw_get_ie23a(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL) + { + DBG_8723A("marc: cannot find SSID for survey event\n"); + return _FAIL; + } + + if (*(p + 1)) { + if (len > IEEE80211_MAX_SSID_LEN) { + DBG_8723A("%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.ssid_len = *(p + 1); + } else { + bssid->Ssid.ssid_len = 0; + } + + memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX); + + /* checking rate info... */ + i = 0; + p = rtw_get_ie23a(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset); + if (p != NULL) + { + if (len > NDIS_802_11_LENGTH_RATES_EX) + { + DBG_8723A("%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_ie23a(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_8723A("%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_ie23a(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_ie23a(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_ch23a(padapter); + } + } + + if (ieee80211_is_probe_req(hdr->frame_control)) { + /* FIXME */ + bssid->InfrastructureMode = Ndis802_11Infrastructure; + ether_addr_copy(bssid->MacAddress, hdr->addr2); + bssid->Privacy = 1; + return _SUCCESS; + } + + memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval23a_from_ie(bssid->IEs), 2); + bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod); + + val16 = rtw_get_capability23a(bssid); + + if (val16 & BIT(0)) { + bssid->InfrastructureMode = Ndis802_11Infrastructure; + ether_addr_copy(bssid->MacAddress, hdr->addr2); + } else { + bssid->InfrastructureMode = Ndis802_11IBSS; + ether_addr_copy(bssid->MacAddress, hdr->addr3); + } + + 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) && (false == pmlmeinfo->bwmode_updated)) + { + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + p = rtw_get_ie23a(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 (pHT_caps->u.HT_cap_element.HT_caps_info & BIT(14)) + pmlmepriv->num_FortyMHzIntolerant++; + } else + { + pmlmepriv->num_sta_no_ht++; + } + } + + + /* mark bss info receving from nearby channel as SignalQuality 101 */ + if (bssid->Configuration.DSConfig != rtw_get_oper_ch23a(padapter)) + bssid->PhyInfo.SignalQuality = 101; + + return _SUCCESS; +} + +void start_create_ibss23a(struct rtw_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 = &pmlmeinfo->network; + pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig; + pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork); + + /* update wireless mode */ + update_wireless_mode23a(padapter); + + /* udpate capability */ + caps = rtw_get_capability23a(pnetwork); + update_capinfo23a(padapter, caps); + if (caps&cap_IBSS)/* adhoc master */ + { + val8 = 0xcf; + rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* switch channel */ + /* SelectChannel23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */ + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + + beacon_timing_control23a(padapter); + + /* set msr to WIFI_FW_ADHOC_STATE */ + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + Set_MSR23a(padapter, (pmlmeinfo->state & 0x3)); + + /* issue beacon */ + if (send_beacon23a(padapter) == _FAIL) + { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n")); + + report_join_res23a(padapter, -1); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + } + else + { + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress); + join_type = 0; + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + report_join_res23a(padapter, 1); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + } + } + else + { + DBG_8723A("start_create_ibss23a, invalid cap:%x\n", caps); + return; + } +} + +void start_clnt_join23a(struct rtw_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 = &pmlmeinfo->network; + int beacon_timeout; + + pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig; + pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork); + + /* update wireless mode */ + update_wireless_mode23a(padapter); + + /* udpate capability */ + caps = rtw_get_capability23a(pnetwork); + update_capinfo23a(padapter, caps); + if (caps&cap_ESS) { + /* switch channel */ + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + Set_MSR23a(padapter, WIFI_FW_STATION_STATE); + + val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* switch channel */ + /* set_channel_bwmode23a(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_timeout23a(pmlmeinfo->bcn_interval); + set_link_timer(pmlmeext, beacon_timeout); + mod_timer(&padapter->mlmepriv.assoc_timer, jiffies + + msecs_to_jiffies((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_MSR23a(padapter, WIFI_FW_ADHOC_STATE); + + val8 = 0xcf; + rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* switch channel */ + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + beacon_timing_control23a(padapter); + + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + + report_join_res23a(padapter, 1); + } + else + { + /* DBG_8723A("marc: invalid cap:%x\n", caps); */ + return; + } +} + +void start_clnt_auth23a(struct rtw_adapter* padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + del_timer_sync(&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_deauth23a(padapter, (&pmlmeinfo->network)->MacAddress, WLAN_REASON_DEAUTH_LEAVING); + + DBG_8723A_LEVEL(_drv_always_, "start auth\n"); + issue_auth23a(padapter, NULL, 0); + + set_link_timer(pmlmeext, REAUTH_TO); +} + +void start_clnt_assoc23a(struct rtw_adapter* padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + del_timer_sync(&pmlmeext->link_timer); + + pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE)); + pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE); + + issue_assocreq23a(padapter); + + set_link_timer(pmlmeext, REASSOC_TO); +} + +unsigned int receive_disconnect23a(struct rtw_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 (!ether_addr_equal(MacAddr, get_my_bssid23a(&pmlmeinfo->network))) + return _SUCCESS; + + DBG_8723A("%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_event23a(padapter, MacAddr, reason); + + } + else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) + { + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res23a(padapter, -2); + } + } + + return _SUCCESS; +} + +static void process_80211d(struct rtw_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_ie23a(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 = j = 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++; + } + } + + if (pregistrypriv->wireless_mode & WIRELESS_11A) { + do { + if ((i == MAX_CHANNEL_NUM) || + (chplan_sta[i].ChannelNum == 0)) + break; + + if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0)) + 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 = chplan_sta[i].ScanType; */ + 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_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] != 0)) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + chplan_new[k].ScanType = SCAN_ACTIVE; + j++; + k++; + } + } else { + /* 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) { + /* 5G Bnad 2, 3 (DFS) doesn't change to active scan */ + if (channel >= 52 && channel <= 144) + break; + + 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_event23a(struct rtw_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; + + if (!padapter) + return; + + pmlmeext = &padapter->mlmeextpriv; + pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!pcmd_obj) + return; + + cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = kzalloc(cmdsz, GFP_ATOMIC); + if (!pevtcmd) { + 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_info23a(padapter, precv_frame, &psurvey_evt->bss) == _FAIL) { + kfree(pcmd_obj); + kfree(pevtcmd); + return; + } + + process_80211d(padapter, &psurvey_evt->bss); + + rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj); + + pmlmeext->sitesurvey_res.bss_cnt++; + + return; +} + +void report_surveydone_event23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!pcmd_obj) + return; + + cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = kzalloc(cmdsz, GFP_ATOMIC); + if (!pevtcmd) { + 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_8723A("survey done event(%x)\n", psurveydone_evt->bss_cnt); + + rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj); + + return; +} + +void report_join_res23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!pcmd_obj) + return; + + cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = kzalloc(cmdsz, GFP_ATOMIC); + if (!pevtcmd) { + 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 = pjoinbss_evt->network.aid = res; + + DBG_8723A("report_join_res23a(%d)\n", res); + + rtw_joinbss_event_prehandle23a(padapter, (u8 *)&pjoinbss_evt->network); + + rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj); + + return; +} + +void report_del_sta_event23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!pcmd_obj) + return; + + cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = kzalloc(cmdsz, GFP_ATOMIC); + if (!pevtcmd) { + 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)); + ether_addr_copy((unsigned char *)&pdel_sta_evt->macaddr, MacAddr); + memcpy((unsigned char *)pdel_sta_evt->rsvd, (unsigned char *)&reason, + 2); + + psta = rtw_get_stainfo23a(&padapter->stapriv, MacAddr); + if (psta) + mac_id = (int)psta->mac_id; + else + mac_id = (-1); + + pdel_sta_evt->mac_id = mac_id; + + DBG_8723A("report_del_sta_event23a: delete STA, mac_id =%d\n", mac_id); + + rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj); + + return; +} + +void report_add_sta_event23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!pcmd_obj) + return; + + cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = kzalloc(cmdsz, GFP_ATOMIC); + if (!pevtcmd) { + 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)); + ether_addr_copy((unsigned char *)&padd_sta_evt->macaddr, MacAddr); + padd_sta_evt->cam_id = cam_idx; + + DBG_8723A("report_add_sta_event23a: add STA\n"); + + rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj); + + return; +} + +/**************************************************************************** + +Following are the event callback functions + +*****************************************************************************/ + +/* for sta/adhoc mode */ +void update_sta_info23a(struct rtw_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_update23a(padapter, psta); + + /* HT */ + if (pmlmepriv->htpriv.ht_option) + { + psta->htpriv.ht_option = true; + + psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable; + + if (support_short_GI23a(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_callback23a(struct rtw_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_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr); + + /* restore to initial setting. */ + update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode); + + goto exit_mlmeext_joinbss_event_callback23a; + } + + if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) + { + /* for bc/mc */ + psta_bmc = rtw_get_bcmc_stainfo23a(padapter); + if (psta_bmc) + { + pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc; + update_bmc_sta_support_rate23a(padapter, psta_bmc->mac_id); + Update_RA_Entry23a(padapter, psta_bmc); + } + } + + /* turn on dynamic functions */ + Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); + + /* update IOT-releated issue */ + update_IOT_info23a(padapter); + + rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates); + + /* BCN interval */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval)); + + /* udpate capability */ + update_capinfo23a(padapter, pmlmeinfo->capability); + + /* WMM, Update EDCA param */ + WMMOnAssocRsp23a(padapter); + + /* HT */ + HTOnAssocRsp23a(padapter); + + /* Set cur_channel&cur_bwmode&cur_ch_offset */ + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress); + if (psta) /* only for infra. mode */ + { + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + /* DBG_8723A("set_sta_rate23a\n"); */ + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + + /* set per sta rate after updating HT cap. */ + set_sta_rate23a(padapter, psta); + + media_status = (psta->mac_id<<8)|1; /* MACID|OPMODE: 1 means connect */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status); + } + + join_type = 2; + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) + { + /* correcting TSF */ + correct_TSF23a(padapter, pmlmeext); + + /* set_link_timer(pmlmeext, DISCONNECT_TO); */ + } + + rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_CONNECT, 0); + +exit_mlmeext_joinbss_event_callback23a: + DBG_8723A("=>%s\n", __func__); +} + +void mlmeext_sta_add_event_callback23a(struct rtw_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_8723A("%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 */ + { + /* update TSF Value */ + /* update_TSF23a(pmlmeext, pframe, len); */ + + /* correcting TSF */ + correct_TSF23a(padapter, pmlmeext); + + /* start beacon */ + if (send_beacon23a(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_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + } + + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + /* rate radaptive */ + Update_RA_Entry23a(padapter, psta); + + /* update adhoc sta_info */ + update_sta_info23a(padapter, psta); +} + +void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + if (is_client_associated_to_ap23a(padapter) || is_IBSS_empty23a(padapter)) + { + /* set_opmode_cmd(padapter, infra_client_with_mlme); */ + + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL); + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr); + + /* restore to initial setting. */ + update_tx_basic_rate23a(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; + + /* SelectChannel23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */ + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + flush_all_cam_entry23a(padapter); + + pmlmeinfo->state = WIFI_FW_NULL_STATE; + + /* set MSR to no link state -> infra. mode */ + Set_MSR23a(padapter, _HW_STATE_STATION_); + + del_timer_sync(&pmlmeext->link_timer); + } +} + +/**************************************************************************** + +Following are the functions for the timer handlers + +*****************************************************************************/ +void linked23a_rx_sig_stren_disp(struct rtw_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_var23a(padapter, HW_DEF_RA_INFO_DUMP,&mac_id); + + rtw_hal_get_def_var23a(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB); + DBG_8723A("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB); +} + +static u8 chk_ap_is_alive(struct rtw_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_chk23a(struct rtw_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) + linked23a_rx_sig_stren_disp(padapter); + + rtw_hal_sreset_linked_status_check23a(padapter); + + if (is_client_associated_to_ap23a(padapter)) + { + /* linked infrastructure client mode */ + + int tx_chk = _SUCCESS, rx_chk = _SUCCESS; + int rx_chk_limit; + + rx_chk_limit = 4; + + if ((psta = rtw_get_stainfo23a(pstapriv, pmlmeinfo->network.MacAddress)) != NULL) + { + bool is_p2p_enable = false; +#ifdef CONFIG_8723AU_P2P + is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE); +#endif + + if (chk_ap_is_alive(padapter, psta) == false) + 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_ch23a(padapter) != pmlmeext->cur_channel) { + backup_oper_channel = rtw_get_oper_ch23a(padapter); + SelectChannel23a(padapter, pmlmeext->cur_channel); + } + + if (rx_chk != _SUCCESS) + issue_probereq23a_ex23a(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1); + + if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) { + tx_chk = issue_nulldata23a(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) + SelectChannel23a(padapter, backup_oper_channel); + + } else { + if (rx_chk != _SUCCESS) { + if (pmlmeext->retry == 0) { + issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress); + issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress); + issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress); + } + } + + if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) + tx_chk = issue_nulldata23a(padapter, NULL, 0, 1, 0); + } + + if (rx_chk == _FAIL) { + pmlmeext->retry++; + if (pmlmeext->retry > rx_chk_limit) { + DBG_8723A_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n", + FUNC_ADPT_ARG(padapter)); + receive_disconnect23a(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_stainfo23a(pstapriv, passoc_res->network.MacAddress)) != NULL) */ + } + else if (is_client_associated_to_ibss23a(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_event23a(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); + } + } + } + + /* set_link_timer(pmlmeext, DISCONNECT_TO); */ + + } +} + +static void survey_timer_hdl(unsigned long data) +{ + struct rtw_adapter *padapter = (struct rtw_adapter *)data; + struct cmd_obj *ph2c; + struct sitesurvey_parm *psurveyPara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif + + /* issue rtw_sitesurvey_cmd23a */ + if (pmlmeext->sitesurvey_res.state > SCAN_START) { + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) + pmlmeext->sitesurvey_res.channel_idx++; + + if (pmlmeext->scan_abort == true) + { +#ifdef CONFIG_8723AU_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_8723A("%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_8723A("%s idx:%d\n", __func__, + pmlmeext->sitesurvey_res.channel_idx); + } + + pmlmeext->scan_abort = false;/* reset */ + } + + ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!ph2c) + goto exit_survey_timer_hdl; + + psurveyPara = (struct sitesurvey_parm*) + kzalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC); + if (!psurveyPara) { + kfree(ph2c); + goto exit_survey_timer_hdl; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey)); + rtw_enqueue_cmd23a(pcmdpriv, ph2c); + } + +exit_survey_timer_hdl: + return; +} + +static void link_timer_hdl(unsigned long data) +{ + struct rtw_adapter *padapter = (struct rtw_adapter *)data; + /* static unsigned int rx_pkt = 0; */ + /* static u64 tx_cnt = 0; */ + /* 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 (pmlmeinfo->state & WIFI_FW_AUTH_NULL) + { + DBG_8723A("link_timer_hdl:no beacon while connecting\n"); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res23a(padapter, -3); + } + else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) + { + /* re-auth timer */ + if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) + { + /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */ + /* */ + pmlmeinfo->state = 0; + report_join_res23a(padapter, -1); + return; + /* */ + /* else */ + /* */ + /* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */ + /* pmlmeinfo->reauth_count = 0; */ + /* */ + } + + DBG_8723A("link_timer_hdl: auth timeout and try again\n"); + pmlmeinfo->auth_seq = 1; + issue_auth23a(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_res23a(padapter, -2); + return; + } + + DBG_8723A("link_timer_hdl: assoc timeout and try again\n"); + issue_assocreq23a(padapter); + set_link_timer(pmlmeext, REASSOC_TO); + } + + return; +} + +static void addba_timer_hdl(unsigned long data) +{ + struct sta_info *psta = (struct sta_info *)data; + struct ht_priv *phtpriv; + + if (!psta) + return; + + phtpriv = &psta->htpriv; + + if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) + { + if (phtpriv->candidate_tid_bitmap) + phtpriv->candidate_tid_bitmap = 0x0; + + } +} + +void init_addba_retry_timer23a(struct sta_info *psta) +{ + setup_timer(&psta->addba_retry_timer, addba_timer_hdl, + (unsigned long)psta); +} + +void init_mlme_ext_timer23a(struct rtw_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + setup_timer(&pmlmeext->survey_timer, survey_timer_hdl, + (unsigned long)padapter); + + setup_timer(&pmlmeext->link_timer, link_timer_hdl, + (unsigned long)padapter); +} + +u8 NULL_hdl23a(struct rtw_adapter *padapter, u8 *pbuf) +{ + return H2C_SUCCESS; +} + +u8 setopmode_hdl23a(struct rtw_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_hwreg23a(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type)); + /* Set_NETYPE0_MSR(padapter, type); */ + + return H2C_SUCCESS; +} + +u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network; + struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf; + /* u32 initialgain; */ + + if (pparm->InfrastructureMode == Ndis802_11APMode) { +#ifdef CONFIG_8723AU_AP_MODE + + if (pmlmeinfo->state == WIFI_FW_AP_STATE) + { + /* todo: */ + return H2C_SUCCESS; + } +#endif + } + + /* below is for ad-hoc master */ + if (pparm->InfrastructureMode == Ndis802_11IBSS) { + rtw_joinbss_reset23a(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_Flag23a(padapter); + Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); + + /* config the initial gain under linking, need to write the BB registers */ + /* initialgain = 0x1E; */ + /* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */ + + /* cancel link timer */ + del_timer_sync(&pmlmeext->link_timer); + + /* clear CAM */ + flush_all_cam_entry23a(padapter); + + if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */ + return H2C_PARAMETERS_ERROR; + + memcpy(pnetwork, pparm, sizeof(struct wlan_bssid_ex)); + + start_create_ibss23a(padapter); + } + + return H2C_SUCCESS; +} + +u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf) +{ + u8 join_type; + struct ndis_802_11_var_ies * 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 = &pmlmeinfo->network; + struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf; + struct HT_info_element *pht_info; + u32 i; + /* u32 initialgain; */ + /* u32 acparm; */ + + /* check already connecting to AP or not */ + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + { + if (pmlmeinfo->state & WIFI_FW_STATION_STATE) + issue_deauth23a_ex23a(padapter, pnetwork->MacAddress, + WLAN_REASON_DEAUTH_LEAVING, 5, 100); + + pmlmeinfo->state = WIFI_FW_NULL_STATE; + + /* clear CAM */ + flush_all_cam_entry23a(padapter); + + del_timer_sync(&pmlmeext->link_timer); + + /* set MSR to nolink -> infra. mode */ + /* Set_MSR23a(padapter, _HW_STATE_NOLINK_); */ + Set_MSR23a(padapter, _HW_STATE_STATION_); + + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL); + } + + rtw_joinbss_reset23a(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; + /* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */ + + if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */ + return H2C_PARAMETERS_ERROR; + + memcpy(pnetwork, pbuf, sizeof(struct wlan_bssid_ex)); + + /* Check AP vendor to move rtw_joinbss_cmd23a() */ + /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pnetwork->IEs, + pnetwork->IELength); */ + + for (i = sizeof(struct ndis_802_11_fixed_ies); i < pnetwork->IELength;) + { + pIE = (struct ndis_802_11_var_ies *)(pnetwork->IEs + i); + + switch (pIE->ElementID) + { + case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */ + if (!memcmp(pIE->data, WMM_OUI23A, 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 */ + 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 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_8723A("set ch/bw before connected\n"); + } + break; + + default: + break; + } + + i += (pIE->Length + 2); + } + /* disable dynamic functions, such as high power, DIG */ + /* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */ + + /* config the initial gain under linking, need to write the BB + registers */ + /* initialgain = 0x1E; */ + /* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, + (u8 *)(&initialgain)); */ + + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, + pmlmeinfo->network.MacAddress); + join_type = 0; + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + /* cancel link timer */ + del_timer_sync(&pmlmeext->link_timer); + + start_clnt_join23a(padapter); + + return H2C_SUCCESS; +} + +u8 disconnect_hdl23a(struct rtw_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 = &pmlmeinfo->network; + u8 val8; + + if (is_client_associated_to_ap23a(padapter)) + { + issue_deauth23a_ex23a(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100); + } + + /* set_opmode_cmd(padapter, infra_client_with_mlme); */ + + /* pmlmeinfo->state = WIFI_FW_NULL_STATE; */ + + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL); + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr); + + /* restore to initial setting. */ + update_tx_basic_rate23a(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_hwreg23a(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8)); + } + + /* set MSR to no link state -> infra. mode */ + Set_MSR23a(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_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + flush_all_cam_entry23a(padapter); + + del_timer_sync(&pmlmeext->link_timer); + + rtw_free_uc_swdec_pending_queue23a(padapter); + + return H2C_SUCCESS; +} + +static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee80211_channel *out, + u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num) +{ + int i, j; + int scan_ch_num = 0; + 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;ichannel_set, in[i].hw_value)) >= 0 + ) + { + memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel)); + + if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE) + out[j].flags &= IEEE80211_CHAN_NO_IR; + + j++; + } + if (j>= out_num) + break; + } + + /* if out is empty, use channel_set as default */ + if (j == 0) { + for (i = 0;imax_chan_nums;i++) { + out[i].hw_value = pmlmeext->channel_set[i].ChannelNum; + + if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE) + out[i].flags &= IEEE80211_CHAN_NO_IR; + + j++; + } + } + + if (padapter->setband == GHZ_24) { /* 2.4G */ + for (i = 0; i < j ; i++) { + if (out[i].hw_value > 35) + memset(&out[i], 0, + sizeof(struct rtw_ieee80211_channel)); + else + scan_ch_num++; + } + j = scan_ch_num; + } else if (padapter->setband == GHZ_50) { /* 5G */ + for (i = 0; i < j ; i++) { + if (out[i].hw_value > 35) { + memcpy(&out[scan_ch_num++], &out[i], sizeof(struct rtw_ieee80211_channel)); + } + } + j = scan_ch_num; + } else + {} + + return j; +} + +u8 sitesurvey_cmd_hdl23a(struct rtw_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; + + if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) { + /* for first time sitesurvey_cmd */ + rtw_hal_set_hwreg23a(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].ssid_len) { + memcpy(pmlmeext->sitesurvey_res.ssid[i].ssid, + pparm->ssid[i].ssid, IW_ESSID_MAX_SIZE); + pmlmeext->sitesurvey_res.ssid[i].ssid_len = + pparm->ssid[i].ssid_len; + } else { + pmlmeext->sitesurvey_res.ssid[i].ssid_len = 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_ap23a(padapter)) { + pmlmeext->sitesurvey_res.state = SCAN_TXNULL; + + /* switch to correct channel of current network + before issue keep-alive frames */ + if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel) + SelectChannel23a(padapter, pmlmeext->cur_channel); + + issue_nulldata23a(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_Flag23a(padapter); + Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); + + /* config the initial gain under scaning, need to + write the BB registers */ + if ((wdev_to_priv(padapter->rtw_wdev))->p2p_enabled == true) { + initialgain = 0x30; + } else + initialgain = 0x1E; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, + (u8 *)(&initialgain)); + + /* set MSR to no link state */ + Set_MSR23a(padapter, _HW_STATE_NOLINK_); + + val8 = 1; /* under site survey */ + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, + (u8 *)(&val8)); + + pmlmeext->sitesurvey_res.state = SCAN_PROCESS; + } + + site_survey23a(padapter); + + return H2C_SUCCESS; +} + +u8 setauth_hdl23a(struct rtw_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_hdl23a(struct rtw_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_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) " + "keyid:%d\n", pparm->algorithm, pparm->keyid); + write_cam23a(padapter, pparm->keyid, ctrl, null_sta, pparm->key); + + /* allow multicast packets to driver */ + padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr); + + return H2C_SUCCESS; +} + +u8 set_stakey_hdl23a(struct rtw_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_8723A_LEVEL(_drv_always_, "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_entry23a(padapter, pparm->id); + return H2C_SUCCESS_RSP; + } + + psta = rtw_get_stainfo23a(pstapriv, pparm->addr); + if (psta) + { + ctrl = (BIT(15) | ((pparm->algorithm) << 2)); + + DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm =%d\n", pparm->algorithm); + + if ((psta->mac_id<1) || (psta->mac_id>(NUM_STA-4))) + { + DBG_8723A("r871x_set_stakey_hdl23a():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_8723A("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_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key); + + return H2C_SUCCESS_RSP; + + } + else + { + DBG_8723A("r871x_set_stakey_hdl23a(): sta has been free\n"); + return H2C_REJECTED; + } + + } + + /* below for sta mode */ + + if (pparm->algorithm == _NO_PRIVACY_) /* clear cam entry */ + { + clear_cam_entry23a(padapter, pparm->id); + return H2C_SUCCESS; + } + + ctrl = BIT(15) | ((pparm->algorithm) << 2); + + write_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key); + + pmlmeinfo->enc_algo = pparm->algorithm; + + return H2C_SUCCESS; +} + +u8 add_ba_hdl23a(struct rtw_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_stainfo23a(&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_BA23a(padapter, pparm->addr, + WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid); + mod_timer(&psta->addba_retry_timer, + jiffies + msecs_to_jiffies(ADDBA_TO)); + } else { + psta->htpriv.candidate_tid_bitmap &= ~CHKBIT(pparm->tid); + } + return H2C_SUCCESS; +} + +u8 set_tx_beacon_cmd23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + ptxBeacon_parm = (struct Tx_Beacon_param *) + kzalloc(sizeof(struct Tx_Beacon_param), GFP_ATOMIC); + if (!ptxBeacon_parm) { + 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_cmd23a(pcmdpriv, ph2c); + +exit: + + + + return res; +} + +u8 mlme_evt_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf) +{ + u8 evt_code, evt_seq; + u16 evt_sz; + uint *peventbuf; + void (*event_callback)(struct rtw_adapter *dev, u8 *pbuf); + struct evt_priv *pevt_priv = &padapter->evtpriv; + + peventbuf = (uint*)pbuf; + evt_sz = (u16)(*peventbuf&0xffff); + evt_seq = (u8)((*peventbuf>>24)&0x7f); + 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_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf) +{ + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + return H2C_SUCCESS; +} + +u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf) +{ + if (send_beacon23a(padapter) == _FAIL) + { + DBG_8723A("issue_beacon23a, fail!\n"); + return H2C_PARAMETERS_ERROR; + } +#ifdef CONFIG_8723AU_AP_MODE + else /* tx bc/mc frames after update TIM */ + { + struct sta_info *psta_bmc; + struct list_head *plist, *phead, *ptmp; + struct xmit_frame *pxmitframe; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo23a(padapter); + if (!psta_bmc) + return H2C_SUCCESS; + + if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0)) + { + msleep(10);/* 10ms, ATIM(HIQ) Windows */ + /* spin_lock_bh(&psta_bmc->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + phead = get_list_head(&psta_bmc->sleep_q); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, + struct xmit_frame, + list); + + 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 */ + + rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe); + } + + /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + } + + } +#endif + + return H2C_SUCCESS; +} + +u8 set_ch_hdl23a(struct rtw_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_8723A(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_bwmode23a(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw); + + return H2C_SUCCESS; +} + +u8 set_chplan_hdl23a(struct rtw_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_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf) +{ + struct LedBlink_param *ledBlink_param; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + ledBlink_param = (struct LedBlink_param *)pbuf; + + return H2C_SUCCESS; +} + +u8 set_csa_hdl23a(struct rtw_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_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf) +{ + return H2C_REJECTED; +} diff --git a/drivers/staging/rtl8723au/core/rtw_p2p.c b/drivers/staging/rtl8723au/core/rtw_p2p.c new file mode 100644 index 000000000000..becc3feaca1d --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_p2p.c @@ -0,0 +1,3963 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_P2P_C_ + +#include +#include +#include + +#ifdef CONFIG_8723AU_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 int is_any_client_associated(struct rtw_adapter *padapter) +{ + return padapter->stapriv.asoc_list_cnt ? true : false; +} + +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; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_8723A("%s\n", __func__); + + pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_ATOMIC); + + pstart = pdata_attr; + pcur = pdata_attr; + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + + list_for_each(plist, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + 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_content23a(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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_DISC_REQUEST; + u8 dialogToken = 0; + + DBG_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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 ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + /* there is no IE in this P2P action frame */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_DEVDISC_RESP; + u8 p2pie[8] = { 0x00 }; + u32 p2pielen = 0; + + DBG_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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 ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P public action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(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_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status); + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method) +{ + struct rtw_adapter *padapter = pwdinfo->padapter; + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u8 dialogToken = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PROVISION_DISC_RESP; + u8 wpsie[ 100 ] = { 0x00 }; + u8 wpsielen = 0; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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 ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + wpsielen = 0; + /* WPS OUI */ + /* u32*) (wpsie) = cpu_to_be32(WPSOUI); */ + RTW_PUT_BE32(wpsie, WPSOUI); + wpsielen += 4; + + /* Config Method */ + /* Type: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); */ + RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); */ + RTW_PUT_BE16(wpsie + wpsielen, 0x0002); + wpsielen += 2; + + /* Value: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(config_method); */ + RTW_PUT_BE16(wpsie + wpsielen, config_method); + wpsielen += 2; + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(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 ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */ + u32 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_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *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 ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(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_content23a(&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_content23a(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content); + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, + &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u16 capability = 0; + 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 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); + + capability = cpu_to_le16(capability); + + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability); + + /* P2P Device ID ATTR */ + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr); + + /* Notice of Absence ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + /* go_add_noa_attr(pwdinfo); */ + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +#ifdef CONFIG_8723AU_P2P +u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the beacon frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (P2P_ROLE_GO == pwdinfo->role) + { + if (is_any_client_associated(pwdinfo->padapter)) + { + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD); + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + } + + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (1 == pwdinfo->wfd_tdls_enable) + { + /* WFD primary sink + available for WFD session + WiFi TDLS mode + WSC (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD | + WFD_DEVINFO_PC_TDLS); + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSC (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe response frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + /* 4. WFD Session Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode */ + + if (true == pwdinfo->session_available) + { + if (P2P_ROLE_GO == pwdinfo->role) + { + if (is_any_client_associated(pwdinfo->padapter)) + { + if (pwdinfo->wfd_tdls_enable) + { + /* TDLS mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT); + } + else + { + /* WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT); + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + /* available for WFD session + TDLS mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT); + } + else + { + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT); + } + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT); + } + else + { + + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT); + } + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT); + } + else + { + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT); + } + + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = NULL; + struct mlme_priv *pmlmepriv = NULL; + struct wifi_display_info *pwfd_info = NULL; + + /* WFD OUI */ + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) + { + return 0; + } + + padapter = pwdinfo->padapter; + pmlmepriv = &padapter->mlmepriv; + pwfd_info = padapter->wdinfo.wfd_info; + + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +#endif /* CONFIG_8723AU_P2P */ + +u32 build_probe_resp_p2p_ie23a(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: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* go_add_noa_attr(pwdinfo); */ + } + + /* 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_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_prov_disc_request_p2p_ie23a(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_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_assoc_resp_p2p_ie23a(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_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code); + + /* Extended Listen Timing ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u32 len = 0; + + return len; +} + +u32 process_probe_req_p2p_ie23a(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_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt, + len - sizeof(struct ieee80211_hdr_3addr) - _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_dispatcher23a function) */ + /* 5. Requested Device Type in WSC IE. (Todo) */ + /* 6. Device ID attribute in P2P IE. (Todo) */ + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len, + len - sizeof(struct ieee80211_hdr_3addr) - _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)) + { + if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen))) + { + if ((p) && !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_ie23a(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 ie_offset; + u8 * ies; + u32 ies_len; + u8 * p2p_ie; + u32 p2p_ielen = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + + if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + return P2P_STATUS_FAIL_REQUEST_UNABLE; + + if (ieee80211_is_assoc_req(hdr->frame_control)) + ie_offset = _ASOCREQ_IE_OFFSET_; + else /* WIFI_REASSOCREQ */ + ie_offset = _REASOCREQ_IE_OFFSET_; + + ies = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset; + ies_len = len - sizeof(struct ieee80211_hdr_3addr) - ie_offset; + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + + if (!p2p_ie) + { + DBG_8723A("[%s] P2P IE not Found!!\n", __func__); + status_code = P2P_STATUS_FAIL_INVALID_PARAM; + } + else + { + DBG_8723A("[%s] P2P IE Found!!\n", __func__); + } + + while (p2p_ie) + { + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen)) + { + DBG_8723A("[%s] Got P2P Capability Attr!!\n", __func__); + cap_attr = le16_to_cpu(cap_attr); + psta->dev_cap = cap_attr&0xff; + } + + /* Check Extended Listen Timing ATTR */ + + /* Check P2P Device Info ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen)) + { + DBG_8723A("[%s] Got P2P DEVICE INFO Attr!!\n", __func__); + pattr_content = pbuf = kzalloc(attr_contentlen, + GFP_ATOMIC); + if (pattr_content) { + u8 num_of_secdev_type; + u16 dev_name_len; + + rtw_get_p2p_attr23a_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(&psta->config_methods, pattr_content, 2);/* Config Methods */ + psta->config_methods = be16_to_cpu(psta->config_methods); + + 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); + } + + /* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */ + psta->dev_name_len = 0; + if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content)) + { + dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2)); + + psta->dev_name_len = (sizeof(psta->dev_name)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_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return status_code; +} + +u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, + uint len) +{ + u8 *frame_body; + u8 status, dialogToken; + struct sta_info *psta = NULL; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *p2p_ie; + u32 p2p_ielen = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe; + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + dialogToken = frame_body[7]; + status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP; + + if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + len - _PUBLIC_ACTION_IE_OFFSET_, NULL, + &p2p_ielen))) { + u8 groupid[38] = { 0x00 }; + u8 dev_addr[ETH_ALEN] = { 0x00 }; + u32 attr_contentlen = 0; + + if (rtw_get_p2p_attr23a_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_attr23a_content(p2p_ie, p2p_ielen, + P2P_ATTR_DEVICE_ID, + dev_addr, + &attr_contentlen)) { + struct list_head *phead, *plist, *ptmp; + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) && + !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) + { + /* spin_unlock_bh(&pstapriv->asoc_list_lock); */ + /* issue GO Discoverability Request */ + issue_group_disc_req(pwdinfo, psta->hwaddr); + /* spin_lock_bh(&pstapriv->asoc_list_lock); */ + 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, hdr->addr2, status, dialogToken); + + return (status == P2P_STATUS_SUCCESS) ? true:false; +} + +u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + return true; +} + +u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo, + u8 *pframe, uint len) +{ + u8 *frame_body; + u8 *wpsie; + u8 *ptr = NULL; + uint wps_ielen = 0, attr_contentlen = 0; + u16 uconfig_method = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + + frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr)); + + wpsie = rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + len - _PUBLIC_ACTION_IE_OFFSET_, NULL, + &wps_ielen); + if (!wpsie) + goto out; + + if (!rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, + (u8 *)&uconfig_method, &attr_contentlen)) + goto out; + + uconfig_method = be16_to_cpu(uconfig_method); + ptr = pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req; + + switch (uconfig_method) + { + case WPS_CM_DISPLYA: + memcpy(ptr, "dis", 3); + break; + + case WPS_CM_LABEL: + memcpy(ptr, "lab", 3); + break; + + case WPS_CM_PUSH_BUTTON: + memcpy(ptr, "pbc", 3); + break; + + case WPS_CM_KEYPAD: + memcpy(ptr, "pad", 3); + break; + } + issue_p2p_provision_resp(pwdinfo, hdr->addr2, frame_body, + uconfig_method); + +out: + DBG_8723A("[%s] config method = %s\n", __func__, ptr); + + return true; +} + +u8 process_p2p_provdisc_resp23a(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_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + struct rtw_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; +#ifdef CONFIG_8723AU_P2P + u8 wfd_ie[ 128 ] = { 0x00 }; + u32 wfd_ielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((wpsie = rtw_get_wps_ie23a(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen))) + { + /* 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_content23a(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len); + wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id); + + 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_8723A("[%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_ie23a(ies, ies_len, NULL, &p2p_ielen); + + if (!p2p_ie) + { + DBG_8723A("[%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; + u16 cap_attr; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING); + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen)) + cap_attr = le16_to_cpu(cap_attr); + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) + { + DBG_8723A("[%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_attr23a_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_attr23a_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_8723A("[%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_attr23a_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_8723A("[%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_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel); + } + } + + } + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + } + +#ifdef CONFIG_8723AU_P2P + /* Added by Albert 20110823 */ + /* Try to get the TCP port information when receiving the negotiation request. */ + if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen)) + { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] WFD IE Found!!\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) + { + pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + } + } +#endif /* CONFIG_8723AU_P2P */ + + return result; +} + +u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + struct rtw_adapter *padapter = pwdinfo->padapter; + u8 result = P2P_STATUS_SUCCESS; + u32 p2p_ielen, wps_ielen; + u8 * ies; + u32 ies_len; + u8 * p2p_ie; +#ifdef CONFIG_8723AU_P2P + u8 wfd_ie[ 128 ] = { 0x00 }; + u32 wfd_ielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + 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_ie23a(ies, ies_len, NULL, &wps_ielen)) + { + + } + else + { + DBG_8723A("[%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_ie23a(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 ]; + u16 cap_attr; + 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. */ + { + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen)) + cap_attr = le16_to_cpu(cap_attr); + + rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) + { + DBG_8723A("[%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_attr23a_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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) + { + DBG_8723A("[%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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + DBG_8723A("[%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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) + { + DBG_8723A("[%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_8723A("[%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_attr23a_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_8723A("[%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_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel); + } + } + + } + } + + } + else + { + DBG_8723A("[%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_attr23a_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_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + } + + } + +#ifdef CONFIG_8723AU_P2P + /* Added by Albert 20111122 */ + /* Try to get the TCP port information when receiving the negotiation response. */ + if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen)) + { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] WFD IE Found!!\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) + { + pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + } + } +#endif /* CONFIG_8723AU_P2P */ + + return result; +} + +u8 process_p2p_group_negotation_confirm23a(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_ie23a(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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) + { + DBG_8723A("[%s] Status = %d\n", __func__, attr_content); + result = attr_content; + + if (attr_content == P2P_STATUS_SUCCESS) + { + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + + /* 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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) + { + DBG_8723A("[%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_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + DBG_8723A("[%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_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return result; +} + +u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *frame_body; + u8 dialogToken = 0; + u8 status = P2P_STATUS_SUCCESS; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe; + + frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr)); + + dialogToken = frame_body[6]; + + /* todo: check NoA attribute */ + + issue_p2p_presence_resp(pwdinfo, hdr->addr2, status, dialogToken); + + return true; +} + +static void find_phase_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct cfg80211_ssid ssid; + u8 _status = 0; + + + + memset((unsigned char*)&ssid, 0, sizeof(struct cfg80211_ssid)); + memcpy(ssid.ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN); + ssid.ssid_len = P2P_WILDCARD_SSID_LEN; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH); + + spin_lock_bh(&pmlmepriv->lock); + _status = rtw_sitesurvey_cmd23a(padapter, &ssid, 1, NULL, 0); + spin_unlock_bh(&pmlmepriv->lock); + + +} + +void p2p_concurrent_handler(struct rtw_adapter* padapter); + +static void restore_p2p_state_handler(struct rtw_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_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + } +} + +static void pre_tx_invitereq_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + set_channel_bwmode23a(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)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void pre_tx_provdisc_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + + set_channel_bwmode23a(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void pre_tx_negoreq_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + + set_channel_bwmode23a(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void ro_ch_handler(struct rtw_adapter *padapter) +{ + struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel) { + if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) + pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel; + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, + HAL_PRIME_CHNL_OFFSET_DONT_CARE, + HT_CHANNEL_WIDTH_20); + } + + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + + pcfg80211_wdinfo->is_ro_ch = false; + + DBG_8723A("cfg80211_remain_on_channel_expired\n"); + + rtw_cfg80211_remain_on_channel_expired(padapter, + pcfg80211_wdinfo->remain_on_ch_cookie, + &pcfg80211_wdinfo->remain_on_ch_channel, + pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL); +} + +static void ro_ch_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + + p2p_protocol_wk_cmd23a(adapter, P2P_RO_CH_WK); +} + +#ifdef CONFIG_8723AU_P2P +void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32* len) +{ + unsigned char *frame_body; + u8 category, action, OUI_Subtype, dialogToken = 0; + u32 wfdielen = 0; + + frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr)); + category = frame_body[0]; + + if (category == WLAN_CATEGORY_PUBLIC) { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR && + !memcmp(frame_body+2, P2P_OUI23A, 4)) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + switch (OUI_Subtype)/* OUI Subtype */ { + case P2P_GO_NEGO_REQ: + wfdielen = build_nego_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_GO_NEGO_RESP: + wfdielen = build_nego_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_GO_NEGO_CONF: + wfdielen = build_nego_confirm_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_INVIT_REQ: + wfdielen = build_invitation_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_INVIT_RESP: + wfdielen = build_invitation_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_DEVDISC_REQ: + break; + case P2P_DEVDISC_RESP: + break; + case P2P_PROVISION_DISC_REQ: + wfdielen = build_provdisc_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_PROVISION_DISC_RESP: + wfdielen = build_provdisc_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + default: + break; + } + } + } else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + } else { + DBG_8723A("%s, action frame category =%d\n", __func__, category); + } +} +#endif + +int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf, u32 len, u8 tx) +{ + int is_p2p_frame = (-1); + unsigned char *frame_body; + u8 category, action, OUI_Subtype, dialogToken = 0; + u8 *p2p_ie = NULL; + uint p2p_ielen = 0; + struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev); + + frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr)); + category = frame_body[0]; + /* just for check */ + if (category == WLAN_CATEGORY_PUBLIC) + { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR && + !memcmp(frame_body+2, P2P_OUI23A, 4)) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + is_p2p_frame = OUI_Subtype; + p2p_ie = rtw_get_p2p_ie23a( + (u8 *)buf+sizeof(struct ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_, + len-sizeof(struct ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_, + NULL, &p2p_ielen); + + switch (OUI_Subtype) {/* OUI Subtype */ + u8 *cont; + uint cont_len; + case P2P_GO_NEGO_REQ: + DBG_8723A("RTW_%s:P2P_GO_NEGO_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + case P2P_GO_NEGO_RESP: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_GO_NEGO_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + + if (!tx) + pwdev_priv->provdisc_req_issued = false; + break; + case P2P_GO_NEGO_CONF: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_GO_NEGO_CONF, dialogToken =%d, status:%d\n", + (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + break; + case P2P_INVIT_REQ: + { + struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info; + int flags = -1; + int op_ch = 0; + + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len))) + flags = *cont; + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len))) + op_ch = *(cont+4); + + if (invit_info->token != dialogToken) + rtw_wdev_invit_info_init(invit_info); + + invit_info->token = dialogToken; + invit_info->flags = (flags ==-1) ? 0x0 : flags; + invit_info->req_op_ch = op_ch; + + DBG_8723A("RTW_%s:P2P_INVIT_REQ, dialogToken =%d, flags:0x%02x, op_ch:%d\n", + (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch); + break; + } + case P2P_INVIT_RESP: + { + struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info; + int status = -1; + int op_ch = 0; + + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len))) + status = *cont; + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len))) + op_ch = *(cont+4); + + if (invit_info->token != dialogToken) { + rtw_wdev_invit_info_init(invit_info); + } else { + invit_info->token = 0; + invit_info->status = (status ==-1) ? 0xff : status; + invit_info->rsp_op_ch = op_ch; + } + + DBG_8723A("RTW_%s:P2P_INVIT_RESP, dialogToken =%d, status:%d, op_ch:%d\n", + (tx == true)?"Tx":"Rx", dialogToken, status, op_ch); + break; + } + case P2P_DEVDISC_REQ: + DBG_8723A("RTW_%s:P2P_DEVDISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + case P2P_DEVDISC_RESP: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_DEVDISC_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + break; + case P2P_PROVISION_DISC_REQ: + { + size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr); + u8 *p2p_ie; + uint p2p_ielen = 0; + uint contentlen = 0; + + DBG_8723A("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + + pwdev_priv->provdisc_req_issued = false; + + p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, + NULL, &p2p_ielen); + if (p2p_ie) { + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen)) + pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */ + else + pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */ + } + } + break; + case P2P_PROVISION_DISC_RESP: + DBG_8723A("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + default: + DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"Tx":"Rx", OUI_Subtype, dialogToken); + break; + } + + } + + } + else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) + { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + + is_p2p_frame = OUI_Subtype; + + switch (OUI_Subtype) { + case P2P_NOTICE_OF_ABSENCE: + DBG_8723A("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_PRESENCE_REQUEST: + DBG_8723A("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_PRESENCE_RESPONSE: + DBG_8723A("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_GO_DISC_REQUEST: + DBG_8723A("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + default: + DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"TX":"RX", OUI_Subtype, dialogToken); + break; + } + + } else { + DBG_8723A("RTW_%s:action frame category =%d\n", (tx == true)?"TX":"RX", category); + } + return is_p2p_frame; +} + +void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter) +{ + struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; + + memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info)); + + setup_timer(&pcfg80211_wdinfo->remain_on_ch_timer, + ro_ch_timer_process, (unsigned long)padapter); +} + +void p2p_protocol_wk_hdl23a(struct rtw_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; + case P2P_RO_CH_WK: + ro_ch_handler(padapter); + break; + } +} + +#ifdef CONFIG_8723AU_P2P +void process_p2p_ps_ie23a(struct rtw_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_ie23a(ies, ies_len, NULL, &p2p_ielen); + + while(p2p_ie) + { + find_p2p = true; + /* Get Notice of Absence IE. */ + if (rtw_get_p2p_attr23a_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 == true) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1); + } + } + else if (pwdinfo->noa_num > 0) + { + pwdinfo->p2p_ps_mode = P2P_PS_NOA; + p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1); + } + else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1); + } + } + + break; /* find target, just break. */ + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + if (find_p2p == true) + { + if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == false)) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1); + } + } + + +} + +void p2p_ps_wk_hdl23a(struct rtw_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_hwreg23a(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 == true) + { + if (pwrpriv->smart_ps == 0) + { + pwrpriv->smart_ps = 2; + rtw_hal_set_hwreg23a(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_8723A("%s(): Enter CTW, change SmartPS\n", __func__); + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode); + } + } + rtw_hal_set_hwreg23a(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_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + } + break; + default: + break; + } + + +} + +u8 p2p_ps_wk_cmd23a(struct rtw_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 *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_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_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_cmd23a(pcmdpriv, ph2c); + } + else + { + p2p_ps_wk_hdl23a(padapter, p2p_ps_state); + } + +exit: + + + + return res; +} +#endif /* CONFIG_8723AU_P2P */ + +static void reset_ch_sitesurvey_timer_process(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + DBG_8723A("[%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; +} + +static void reset_ch_sitesurvey_timer_process2(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + DBG_8723A("[%s] In\n", __func__); + /* Reset the operation channel information */ + pwdinfo->p2p_info.operation_ch[0] = 0; + pwdinfo->p2p_info.scan_op_ch_only = 0; +} + +static void restore_p2p_state_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + p2p_protocol_wk_cmd23a(adapter, P2P_RESTORE_STATE_WK); +} + +static void pre_tx_scan_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + 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 (true == pwdinfo->tx_prov_disc_info.benable) /* the provision discovery request frame is trigger to send or not */ + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK); + /* issue23a_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 (true == pwdinfo->nego_req_info.benable) + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK); + } + } + else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) + { + if (true == pwdinfo->invitereq_info.benable) + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK); + } + } + else + { + DBG_8723A("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo)); + } + + spin_unlock_bh(&pmlmepriv->lock); +} + +static void find_phase_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + 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_cmd23a(adapter, P2P_FIND_PHASE_WK); +} + +void reset_global_wifidirect_info23a(struct rtw_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; +} + +#ifdef CONFIG_8723AU_P2P +int rtw_init_wifi_display_info(struct rtw_adapter* padapter) +{ + int res = _SUCCESS; + struct wifi_display_info *pwfd_info = &padapter->wfd_info; + + /* Used in P2P and TDLS */ + pwfd_info->rtsp_ctrlport = 554; + pwfd_info->peer_rtsp_ctrlport = 0; /* Reset to 0 */ + pwfd_info->wfd_enable = false; + pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK; + pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY; + + /* Used in P2P */ + pwfd_info->peer_session_avail = true; + pwfd_info->wfd_pc = false; + + /* Used in TDLS */ + memset(pwfd_info->ip_address, 0x00, 4); + memset(pwfd_info->peer_ip_address, 0x00, 4); + return res; +} +#endif /* CONFIG_8723AU_P2P */ + +void rtw_init_wifidirect_timers23a(struct rtw_adapter* padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + setup_timer(&pwdinfo->find_phase_timer, find_phase_timer_process, + (unsigned long)padapter); + setup_timer(&pwdinfo->restore_p2p_state_timer, + restore_p2p_state_timer_process, (unsigned long)padapter); + setup_timer(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process, + (unsigned long)padapter); + setup_timer(&pwdinfo->reset_ch_sitesurvey, + reset_ch_sitesurvey_timer_process, (unsigned long)padapter); + setup_timer(&pwdinfo->reset_ch_sitesurvey2, + reset_ch_sitesurvey_timer_process2, + (unsigned long)padapter); +} + +void rtw_init_wifidirect_addrs23a(struct rtw_adapter* padapter, u8 *dev_addr, u8 *iface_addr) +{ +#ifdef CONFIG_8723AU_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_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role) +{ + struct wifidirect_info *pwdinfo; +#ifdef CONFIG_8723AU_P2P + struct wifi_display_info *pwfd_info = &padapter->wfd_info; +#endif + + 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((void*) 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) ((rtw_get_current_time() % 3) + 1); + /* DBG_8723A("[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell); */ + + 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, IEEE80211_MAX_SSID_LEN); + pwdinfo->nego_ssidlen = 0; + + pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO; +#ifdef CONFIG_8723AU_P2P + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC; + pwdinfo->wfd_info = pwfd_info; +#else + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD; +#endif /* CONFIG_8723AU_P2P */ + 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_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + int ret = _SUCCESS; + + 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_rate23a(padapter, WIRELESS_11AGN); + + /* Enable P2P function */ + init_wifidirect_info23a(padapter, role); + + rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, true); + #ifdef CONFIG_8723AU_P2P + rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true); + #endif + + } + 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)) + { + del_timer_sync(&pwdinfo->find_phase_timer); + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + del_timer_sync(&pwdinfo->pre_tx_scan_timer); + del_timer_sync(&pwdinfo->reset_ch_sitesurvey); + del_timer_sync(&pwdinfo->reset_ch_sitesurvey2); + reset_ch_sitesurvey_timer_process((unsigned long)padapter); + reset_ch_sitesurvey_timer_process2((unsigned long)padapter); + 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_var23a(padapter, HAL_ODM_P2P_STATE, NULL, false); + #ifdef CONFIG_8723AU_P2P + rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false); + #endif + + /* Restore to initial setting. */ + update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode); + } + +exit: + return ret; +} + +#endif /* CONFIG_8723AU_P2P */ diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c new file mode 100644 index 000000000000..354873ca344e --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c @@ -0,0 +1,686 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_PWRCTRL_C_ + +#include +#include +#include + +#ifdef CONFIG_8723AU_BT_COEXIST +#include +#endif + +void ips_enter23a(struct rtw_adapter * padapter) +{ + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + + down(&pwrpriv->lock); + + pwrpriv->bips_processing = true; + + /* syn ips_mode with request */ + pwrpriv->ips_mode = pwrpriv->ips_mode_req; + + pwrpriv->ips_enter23a_cnts++; + DBG_8723A("==>ips_enter23a cnts:%d\n", pwrpriv->ips_enter23a_cnts); +#ifdef CONFIG_8723AU_BT_COEXIST + BTDM_TurnOffBtCoexistBeforeEnterIPS(padapter); +#endif + if (rf_off == pwrpriv->change_rfpwrstate) + { + pwrpriv->bpower_saving = true; + DBG_8723A_LEVEL(_drv_always_, "nolinked power save enter\n"); + + if (pwrpriv->ips_mode == IPS_LEVEL_2) + pwrpriv->bkeepfwalive = true; + + rtw_ips_pwr_down23a(padapter); + pwrpriv->rf_pwrstate = rf_off; + } + pwrpriv->bips_processing = false; + + up(&pwrpriv->lock); +} + +int ips_leave23a(struct rtw_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; + + down(&pwrpriv->lock); + + if ((pwrpriv->rf_pwrstate == rf_off) &&!pwrpriv->bips_processing) + { + pwrpriv->bips_processing = true; + pwrpriv->change_rfpwrstate = rf_on; + pwrpriv->ips_leave23a_cnts++; + DBG_8723A("==>ips_leave23a cnts:%d\n", pwrpriv->ips_leave23a_cnts); + + if ((result = rtw_ips_pwr_up23a(padapter)) == _SUCCESS) { + pwrpriv->rf_pwrstate = rf_on; + } + DBG_8723A_LEVEL(_drv_always_, "nolinked power save leave\n"); + + if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) ||(_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm)) + { + DBG_8723A("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing); + set_channel_bwmode23a(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 & CHKBIT(keyid)) { + if (keyid == psecuritypriv->dot11PrivacyKeyIndex) + result = rtw_set_key23a(padapter, psecuritypriv, keyid, 1); + else + result = rtw_set_key23a(padapter, psecuritypriv, keyid, 0); + } + } + } + + DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c)); + pwrpriv->bips_processing = false; + + pwrpriv->bkeepfwalive = false; + pwrpriv->bpower_saving = false; + } + + up(&pwrpriv->lock); + + return result; +} + + +static bool rtw_pwr_unassociated_idle(struct rtw_adapter *adapter) +{ + struct rtw_adapter *buddy = adapter->pbuddy_adapter; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct xmit_priv *pxmit_priv = &adapter->xmitpriv; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &adapter->wdinfo; +#endif + + bool ret = false; + + if (adapter->pwrctrlpriv.ips_deny_time >= rtw_get_current_time()) + 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_AP_STATE) + || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE) + || !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) + ) { + goto exit; + } + + /* consider buddy, if exist */ + if (buddy) { + struct mlme_priv *b_pmlmepriv = &buddy->mlmepriv; +#ifdef CONFIG_8723AU_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) + || !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE) + ) { + goto exit; + } + } + + if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF || + pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) { + DBG_8723A_LEVEL(_drv_always_, "There are some pkts to transmit\n"); + DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n", + pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt); + goto exit; + } + + ret = true; + +exit: + return ret; +} + +void rtw_ps_processor23a(struct rtw_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 == true) + goto exit; + + if (padapter->pwrctrlpriv.bHWPwrPindetect) { + rfpwrstate = RfOnOffDetect23a(padapter); + DBG_8723A("@@@@- #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_suspend23a(padapter); + } else { + pwrpriv->change_rfpwrstate = rf_on; + rtw_hw_resume23a(padapter); + } + DBG_8723A("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) == false) + goto exit; + + if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0)) + { + DBG_8723A("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv)); + pwrpriv->change_rfpwrstate = rf_off; + ips_enter23a(padapter); + } +exit: + rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv); + pwrpriv->ps_processing = false; + return; +} + +static void pwr_state_check_handler(unsigned long data) +{ + struct rtw_adapter *padapter = (struct rtw_adapter *)data; + rtw_ps_cmd23a(padapter); +} + +/* + * + * Parameters + * padapter + * pslv power state level, only could be PS_STATE_S0 ~ PS_STATE_S4 + * + */ +void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv) +{ + u8 rpwm; + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + + + + pslv = PS_STATE(pslv); + + if (true == 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 == true) || + (padapter->hw_init_completed == false)) { + 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 == true) { + 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_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm)); + + pwrpriv->rpwm = pslv; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm)); + + pwrpriv->tog += 0x80; + pwrpriv->cpwm = pslv; + + +} + +u8 PS_RDY_CHECK(struct rtw_adapter * padapter) +{ + unsigned long delta_time; + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + delta_time = jiffies - pwrpriv->DelayLPSLastTimeStamp; + + if (delta_time < LPS_DELAY_TIME) + { + return false; + } + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == false) || + (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) || + (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) + return false; + if (true == pwrpriv->bInSuspend) + return false; + if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false)) + { + DBG_8723A("Group handshake still in progress !!!\n"); + return false; + } + if (!rtw_cfg80211_pwr_mgmt(padapter)) + return false; + + return true; +} + +void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode) +{ + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif /* CONFIG_8723AU_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 (ps_mode == PS_MODE_ACTIVE) { +#ifdef CONFIG_8723AU_P2P + if (pwdinfo->opp_ps == 0) +#endif /* CONFIG_8723AU_P2P */ + { + DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n"); + + pwrpriv->pwr_mode = ps_mode; + rtw_set_rpwm23a(padapter, PS_STATE_S4); + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + pwrpriv->bFwCurrentInPSMode = false; + } + } + else + { + if (PS_RDY_CHECK(padapter) +#ifdef CONFIG_8723AU_BT_COEXIST + || (BT_1Ant(padapter) == true) +#endif + ) + { + DBG_8723A("%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_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + +#ifdef CONFIG_8723AU_P2P + /* Set CTWindow after LPS */ + if (pwdinfo->opp_ps == 1) + p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 0); +#endif /* CONFIG_8723AU_P2P */ + + rtw_set_rpwm23a(padapter, PS_STATE_S2); + } + } + + +} + +/* + * Return: + * 0: Leave OK + * -1: Timeout + * -2: Other error + */ +s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms) +{ + u32 start_time; + u8 bAwake = false; + s32 err = 0; + + start_time = rtw_get_current_time(); + while (1) + { + rtw23a_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake); + if (true == bAwake) + break; + + if (true == padapter->bSurpriseRemoved) + { + err = -2; + DBG_8723A("%s: device surprise removed!!\n", __func__); + break; + } + + if (rtw_get_passing_time_ms23a(start_time) > delay_ms) + { + err = -1; + DBG_8723A("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms); + break; + } + udelay(100); + } + + return err; +} + +/* Description: */ +/* Enter the leisure power save mode. */ +void LPS_Enter23a(struct rtw_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + + if (!PS_RDY_CHECK(padapter)) + 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_8723A("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps); + /* For Tenda W311R IOT issue */ + rtw_set_ps_mode23a(padapter, pwrpriv->power_mgnt, pwrpriv->smart_ps, 0); + } + } else { + pwrpriv->LpsIdleCount++; + } + } +} + +/* Description: */ +/* Leave the leisure power save mode. */ +void LPS_Leave23a(struct rtw_adapter *padapter) +{ +#define LPS_LEAVE_TIMEOUT_MS 100 + + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + + if (pwrpriv->bLeisurePs) { + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + rtw_set_ps_mode23a(padapter, PS_MODE_ACTIVE, 0, 0); + + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) + LPS_RF_ON_check23a(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 LeaveAllPowerSaveMode23a(struct rtw_adapter *Adapter) +{ + struct mlme_priv *pmlmepriv = &Adapter->mlmepriv; + u8 enqueue = 0; + + + + /* DBG_8723A("%s.....\n", __func__); */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { /* connect */ +#ifdef CONFIG_8723AU_P2P + p2p_ps_wk_cmd23a(Adapter, P2P_PS_DISABLE, enqueue); +#endif /* CONFIG_8723AU_P2P */ + + rtw_lps_ctrl_wk_cmd23a(Adapter, LPS_CTRL_LEAVE, enqueue); + } + + +} + +void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter) +{ + struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv; + + sema_init(&pwrctrlpriv->lock, 1); + pwrctrlpriv->rf_pwrstate = rf_on; + pwrctrlpriv->ips_enter23a_cnts = 0; + pwrctrlpriv->ips_leave23a_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; + 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; + + setup_timer(&pwrctrlpriv->pwr_state_check_timer, + pwr_state_check_handler, (unsigned long)padapter); + + +} + +void rtw_free_pwrctrl_priv(struct rtw_adapter *adapter) +{ +} + +u8 rtw_interface_ps_func23a(struct rtw_adapter *padapter, enum hal_intf_ps_func efunc_id, u8* val) +{ + u8 bResult = true; + rtw_hal_intf_ps_func23a(padapter, efunc_id, val); + + return bResult; +} + +inline void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms) +{ + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime23a(ms); +} + +/* +* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend +* @adapter: pointer to _adapter structure +* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup +* Return _SUCCESS or _FAIL +*/ + +int _rtw_pwr_wakeup23a(struct rtw_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 = rtw_get_current_time(); + + if (pwrpriv->ips_deny_time < rtw_get_current_time() + rtw_ms_to_systime23a(ips_deffer_ms)) + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime23a(ips_deffer_ms); + + if (pwrpriv->ps_processing) { + DBG_8723A("%s wait ps_processing...\n", __func__); + while (pwrpriv->ps_processing && rtw_get_passing_time_ms23a(start) <= 3000) + msleep(10); + if (pwrpriv->ps_processing) + DBG_8723A("%s wait ps_processing timeout\n", __func__); + else + DBG_8723A("%s wait ps_processing done\n", __func__); + } + + if (rtw_hal_sreset_inprogress(padapter)) { + DBG_8723A("%s wait sreset_inprogress...\n", __func__); + while (rtw_hal_sreset_inprogress(padapter) && rtw_get_passing_time_ms23a(start) <= 4000) + msleep(10); + if (rtw_hal_sreset_inprogress(padapter)) + DBG_8723A("%s wait sreset_inprogress timeout\n", __func__); + else + DBG_8723A("%s wait sreset_inprogress done\n", __func__); + } + + if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) { + DBG_8723A("%s wait bInSuspend...\n", __func__); + while (pwrpriv->bInSuspend && + (rtw_get_passing_time_ms23a(start) <= 3000)) { + msleep(10); + } + if (pwrpriv->bInSuspend) + DBG_8723A("%s wait bInSuspend timeout\n", __func__); + else + DBG_8723A("%s wait bInSuspend done\n", __func__); + } + + /* System suspend is not allowed to wakeup */ + if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) { + ret = _FAIL; + goto exit; + } + + /* block??? */ + if ((pwrpriv->bInternalAutoSuspend == true) && (padapter->net_closed == true)) { + ret = _FAIL; + goto exit; + } + + /* I think this should be check in IPS, LPS, autosuspend functions... */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + ret = _SUCCESS; + goto exit; + } + + if (rf_off == pwrpriv->rf_pwrstate) { + DBG_8723A("%s call ips_leave23a....\n", __func__); + if (_FAIL == ips_leave23a(padapter)) { + DBG_8723A("======> ips_leave23a fail.............\n"); + ret = _FAIL; + goto exit; + } + } + + /* TODO: the following checking need to be merged... */ + if (padapter->bDriverStopped || !padapter->bup || + !padapter->hw_init_completed) { + DBG_8723A("%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 < rtw_get_current_time() + rtw_ms_to_systime23a(ips_deffer_ms)) + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime23a(ips_deffer_ms); + return ret; +} + +int rtw_pm_set_lps23a(struct rtw_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) + { + LeaveAllPowerSaveMode23a(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_ips23a(struct rtw_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_8723A("%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_8723A("%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/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c new file mode 100644 index 000000000000..0b2455e4f5b6 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_recv.c @@ -0,0 +1,2471 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_RECV_C_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void rtw_signal_stat_timer_hdl23a(unsigned long data); + +void _rtw_init_sta_recv_priv23a(struct sta_recv_priv *psta_recvpriv) +{ + + + + spin_lock_init(&psta_recvpriv->lock); + + /* for (i = 0; iblk_strms[i]); */ + + _rtw_init_queue23a(&psta_recvpriv->defrag_q); + + +} + +int _rtw_init_recv_priv23a(struct recv_priv *precvpriv, + struct rtw_adapter *padapter) +{ + struct recv_frame *precvframe; + int i; + int res = _SUCCESS; + + + + /* We don't need to memset padapter->XXX to zero, because + adapter is allocated by rtw_zvmalloc(). */ + /* memset((unsigned char *)precvpriv, 0, sizeof (struct recv_priv)); */ + + spin_lock_init(&precvpriv->lock); + + _rtw_init_queue23a(&precvpriv->free_recv_queue); + _rtw_init_queue23a(&precvpriv->recv_pending_queue); + _rtw_init_queue23a(&precvpriv->uc_swdec_pending_queue); + + precvpriv->adapter = padapter; + + precvpriv->free_recvframe_cnt = NR_RECVFRAME; + + precvpriv->pallocated_frame_buf = + rtw_zvmalloc(NR_RECVFRAME * sizeof(struct recv_frame)); + + if (precvpriv->pallocated_frame_buf == NULL) { + res = _FAIL; + goto exit; + } + + precvframe = precvpriv->pallocated_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_alloc23a(padapter, precvframe); + + precvframe->adapter = padapter; + precvframe++; + } + + precvpriv->rx_pending_cnt = 1; + + sema_init(&precvpriv->allrxreturnevt, 0); + + res = rtw_hal_init23a_recv_priv(padapter); + + setup_timer(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl23a, + (unsigned long)padapter); + + precvpriv->signal_stat_sampling_interval = 1000; /* ms */ + + rtw_set_signal_stat_timer(precvpriv); + +exit: + + + + return res; +} + +void _rtw_free_recv_priv23a (struct recv_priv *precvpriv) +{ + struct rtw_adapter *padapter = precvpriv->adapter; + + + + rtw_free_uc_swdec_pending_queue23a(padapter); + + if (precvpriv->pallocated_frame_buf) { + rtw_vmfree(precvpriv->pallocated_frame_buf, + NR_RECVFRAME * sizeof(struct recv_frame)); + } + + rtw_hal_free_recv_priv23a(padapter); + + +} + +struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue) +{ + struct recv_frame *pframe; + struct list_head *plist, *phead; + struct rtw_adapter *padapter; + struct recv_priv *precvpriv; + + spin_lock_bh(&pfree_recv_queue->lock); + + if (_rtw_queue_empty23a(pfree_recv_queue) == true) + pframe = NULL; + else { + phead = get_list_head(pfree_recv_queue); + + plist = phead->next; + + pframe = container_of(plist, struct recv_frame, list); + + list_del_init(&pframe->list); + padapter = pframe->adapter; + if (padapter) { + precvpriv = &padapter->recvpriv; + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt--; + } + } + + spin_unlock_bh(&pfree_recv_queue->lock); + + return pframe; +} + +int rtw_free_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *pfree_recv_queue) +{ + struct rtw_adapter *padapter = precvframe->adapter; + struct recv_priv *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); + + list_add_tail(&precvframe->list, get_list_head(pfree_recv_queue)); + + if (padapter) { + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + } + + spin_unlock_bh(&pfree_recv_queue->lock); + + + + return _SUCCESS; +} + +int rtw_enqueue_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *queue) +{ + struct rtw_adapter *padapter = precvframe->adapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + + spin_lock_bh(&queue->lock); + + list_del_init(&precvframe->list); + + list_add_tail(&precvframe->list, get_list_head(queue)); + + if (padapter) { + if (queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + } + + spin_unlock_bh(&queue->lock); + + return _SUCCESS; +} + +/* +caller : defrag ; recvframe_chk_defrag23a in recv_thread (passive) +pframequeue: defrag_queue : will be accessed in recv_thread (passive) + +using spinlock to protect + +*/ + +void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue, struct rtw_queue *pfree_recv_queue) +{ + struct recv_frame *hdr; + struct list_head *plist, *phead, *ptmp; + + + spin_lock(&pframequeue->lock); + + phead = get_list_head(pframequeue); + plist = phead->next; + + list_for_each_safe(plist, ptmp, phead) { + hdr = container_of(plist, struct recv_frame, list); + rtw_free_recvframe23a(hdr, pfree_recv_queue); + } + + spin_unlock(&pframequeue->lock); + + +} + +u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter) +{ + u32 cnt = 0; + struct recv_frame *pending_frame; + while ((pending_frame = rtw_alloc_recvframe23a(&adapter->recvpriv.uc_swdec_pending_queue))) { + rtw_free_recvframe23a(pending_frame, + &adapter->recvpriv.free_recv_queue); + DBG_8723A("%s: dequeue uc_swdec_pending_queue\n", __func__); + cnt++; + } + + return cnt; +} + +int rtw_enqueue_recvbuf23a_to_head(struct recv_buf *precvbuf, struct rtw_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_recvbuf23a(struct recv_buf *precvbuf, struct rtw_queue *queue) +{ + unsigned long irqL; + spin_lock_irqsave(&queue->lock, irqL); + + list_del_init(&precvbuf->list); + + list_add_tail(&precvbuf->list, get_list_head(queue)); + spin_unlock_irqrestore(&queue->lock, irqL); + return _SUCCESS; +} + +struct recv_buf *rtw_dequeue_recvbuf23a (struct rtw_queue *queue) +{ + unsigned long irqL; + struct recv_buf *precvbuf; + struct list_head *plist, *phead; + + spin_lock_irqsave(&queue->lock, irqL); + + if (_rtw_queue_empty23a(queue) == true) { + 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, irqL); + + return precvbuf; +} + +int recvframe_chkmic(struct rtw_adapter *adapter, + struct recv_frame *precvframe); +int recvframe_chkmic(struct rtw_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; + /* u8 *iv, rxdata_key_idx = 0; */ + 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_stainfo23a(&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_multicast_ether_addr(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->binstallGrpkey == false) { + res = _FAIL; + RT_TRACE(_module_rtl871x_recv_c_, + _drv_err_, + ("\n recvframe_chkmic:didn't " + "install group key!!!!!!\n")); + DBG_8723A("\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")); + } + + /* icv_len included the mic code */ + datalen = precvframe->pkt->len-prxattrib-> + hdrlen-prxattrib->iv_len-prxattrib->icv_len - 8; + pframe = precvframe->pkt->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)); + + /* care the length of the data */ + rtw_seccalctkipmic23a(mickey, pframe, payload, + datalen, &miccode[0], + (unsigned char)prxattrib->priority); + + 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 == true) { + int i; + 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))); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + ("\n ====== demp packet (len =%d) ======" + "\n", precvframe->pkt->len)); + for (i = 0; i < precvframe->pkt->len; i = i + 8) { + RT_TRACE(_module_rtl871x_recv_c_, + _drv_err_, ("0x%02x:0x%02x:0x" + "%02x:0x%02x:0x%0" + "2x:0x%02x:0x%02x" + ":0x%02x", + *(precvframe->pkt->data+i),*(precvframe->pkt->data+i+1), + *(precvframe->pkt->data+i+2),*(precvframe->pkt->data+i+3), + *(precvframe->pkt->data+i+4),*(precvframe->pkt->data+i+5), + *(precvframe->pkt->data+i+6),*(precvframe->pkt->data+i+7))); + } + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + ("\n ====== demp packet end [len =%d]" + "======\n", precvframe->pkt->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_multicast_ether_addr(prxattrib->ra)) && + (prxattrib->key_index != + pmlmeinfo->key_index)) + brpt_micerror = false; + + if ((prxattrib->bdecrypted == true) && + (brpt_micerror == true)) { + rtw_handle_tkip_mic_err23a(adapter, (u8)is_multicast_ether_addr(prxattrib->ra)); + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted)); + DBG_8723A(" 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_8723A(" mic error :prxattrib->" + "bdecrypted =%d\n", + prxattrib->bdecrypted); + } + + res = _FAIL; + } else { + /* mic checked ok */ + if ((psecuritypriv->bcheck_grpkey == false) && + (is_multicast_ether_addr(prxattrib->ra))) { + psecuritypriv->bcheck_grpkey = true; + RT_TRACE(_module_rtl871x_recv_c_, + _drv_err_, + ("psecuritypriv->bcheck_grp" + "key = true")); + } + } + } else { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + ("recvframe_chkmic: rtw_get_stainfo23a ==" + "NULL!!!\n")); + } + + skb_trim(precvframe->pkt, precvframe->pkt->len - 8); + } + +exit: + + + + return res; +} + +/* decrypt and set the ivlen, icvlen of the recv_frame */ +struct recv_frame *decryptor(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +struct recv_frame *decryptor(struct rtw_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->pkt->data + prxattrib->hdrlen; + prxattrib->key_index = (((iv[3]) >> 6) & 0x3); + + if (prxattrib->key_index > WEP_KEYS) { + DBG_8723A("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->hw_decrypted = false; + switch (prxattrib->encrypt) { + case _WEP40_: + case _WEP104_: + rtw_wep_decrypt23a(padapter, precv_frame); + break; + case _TKIP_: + res = rtw_tkip_decrypt23a(padapter, precv_frame); + break; + case _AES_: + res = rtw_aes_decrypt23a(padapter, 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_recvframe23a(return_packet, + &padapter->recvpriv.free_recv_queue); + return_packet = NULL; + } + + + + return return_packet; +} + +/* set the security information in the recv_frame */ +static struct recv_frame *portctrl(struct rtw_adapter *adapter, + struct recv_frame *precv_frame) +{ + u8 *psta_addr = NULL, *ptr; + uint auth_alg; + struct recv_frame *pfhdr; + struct sta_info *psta; + struct sta_priv *pstapriv ; + struct recv_frame *prtnframe; + u16 ether_type = 0; + u16 eapol_type = 0x888e;/* for Funia BD's WPA issue */ + struct rx_pkt_attrib *pattrib; + + pstapriv = &adapter->stapriv; + psta = rtw_get_stainfo23a(pstapriv, psta_addr); + + auth_alg = adapter->securitypriv.dot11AuthAlgrthm; + + ptr = precv_frame->pkt->data; + pfhdr = precv_frame; + pattrib = &pfhdr->attrib; + psta_addr = pattrib->ta; + + 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 + LLC_HEADER_SIZE; + memcpy(ðer_type, ptr, 2); + ether_type = ntohs((unsigned short)ether_type); + + if (ether_type == eapol_type) { + prtnframe = precv_frame; + } else { + /* free this frame */ + rtw_free_recvframe23a(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; +} + +int recv_decache(struct recv_frame *precv_frame, u8 bretry, + struct stainfo_rxcache *prxcache); +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 process23a_pwrbit_data(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +void process23a_pwrbit_data(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_AP_MODE + unsigned char pwrbit; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + struct rx_pkt_attrib *pattrib = &precv_frame->attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + + psta = rtw_get_stainfo23a(pstapriv, pattrib->src); + + if (psta) { + pwrbit = ieee80211_has_pm(hdr->frame_control); + + if (pwrbit) { + if (!(psta->state & WIFI_SLEEP_STATE)) + stop_sta_xmit23a(padapter, psta); + } else { + if (psta->state & WIFI_SLEEP_STATE) + wakeup_sta_to_xmit23a(padapter, psta); + } + } + +#endif +} + +void process_wmmps_data(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +void process_wmmps_data(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_AP_MODE + struct rx_pkt_attrib *pattrib = &precv_frame->attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + + psta = rtw_get_stainfo23a(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_frames23a(padapter, psta); + } else { + /* issue one qos null frame with More data bit = 0 and the EOSP bit set (= 1) */ + issue_qos_nulldata23a(padapter, psta->hwaddr, + (u16)pattrib->priority, + 0, 0); + } + } + } + +#endif +} + +static void count_rx_stats(struct rtw_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 = prframe->pkt->len; + precvpriv->rx_bytes += sz; + + padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++; + + if ((!is_broadcast_ether_addr(pattrib->dst)) && + (!is_multicast_ether_addr(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; + } +} + +static int sta2sta_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame, + struct sta_info**psta) +{ + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->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_multicast_ether_addr(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 (ether_addr_equal(myhwaddr, pattrib->src)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + (" SA == myself\n")); + ret = _FAIL; + goto exit; + } + + if (!ether_addr_equal(myhwaddr, pattrib->dst) && !bmcast) { + ret = _FAIL; + goto exit; + } + + if (ether_addr_equal(pattrib->bssid, "\x0\x0\x0\x0\x0\x0") || + ether_addr_equal(mybssid, "\x0\x0\x0\x0\x0\x0") || + !ether_addr_equal(pattrib->bssid, mybssid)) { + ret = _FAIL; + goto exit; + } + + sta_addr = pattrib->src; + } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + /* For Station mode, sa and bssid should always be BSSID, + and DA is my mac-address */ + if (!ether_addr_equal(pattrib->bssid, pattrib->src)) { + 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) == true) { + if (bmcast) { + /* For AP mode, if DA == MCAST, then BSSID should be also MCAST */ + if (!is_multicast_ether_addr(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 (!ether_addr_equal(pattrib->bssid, pattrib->dst)) { + ret = _FAIL; + goto exit; + } + + sta_addr = pattrib->src; + } + } else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) { + ether_addr_copy(pattrib->dst, hdr->addr1); + ether_addr_copy(pattrib->src, hdr->addr2); + ether_addr_copy(pattrib->bssid, hdr->addr3); + ether_addr_copy(pattrib->ra, pattrib->dst); + ether_addr_copy(pattrib->ta, pattrib->src); + + sta_addr = mybssid; + } else { + ret = _FAIL; + } + + if (bmcast) + *psta = rtw_get_bcmc_stainfo23a(adapter); + else + *psta = rtw_get_stainfo23a(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")); + ret = _FAIL; + goto exit; + } + +exit: + + return ret; +} + +int ap2sta_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame, + struct sta_info **psta); +int ap2sta_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame, + struct sta_info **psta) +{ + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->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_multicast_ether_addr(pattrib->dst); + + + + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true || + check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)) { + + /* filter packets that SA is myself or multicast or broadcast */ + if (ether_addr_equal(myhwaddr, pattrib->src)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + (" SA == myself\n")); + ret = _FAIL; + goto exit; + } + + /* da should be for me */ + if (!ether_addr_equal(myhwaddr, pattrib->dst) && !bmcast) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + (" ap2sta_data_frame: compare DA fail; DA =" + MAC_FMT"\n", MAC_ARG(pattrib->dst))); + ret = _FAIL; + goto exit; + } + + /* check BSSID */ + if (ether_addr_equal(pattrib->bssid, "\x0\x0\x0\x0\x0\x0") || + ether_addr_equal(mybssid, "\x0\x0\x0\x0\x0\x0") || + !ether_addr_equal(pattrib->bssid, mybssid)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + (" ap2sta_data_frame: compare BSSID fail ; " + "BSSID ="MAC_FMT"\n", MAC_ARG(pattrib->bssid))); + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + ("mybssid ="MAC_FMT"\n", MAC_ARG(mybssid))); + + if (!bmcast) { + DBG_8723A("issue_deauth23a to the nonassociated " + "ap =" MAC_FMT " for the reason(7)\n", + MAC_ARG(pattrib->bssid)); + issue_deauth23a(adapter, pattrib->bssid, + WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + + ret = _FAIL; + goto exit; + } + + if (bmcast) + *psta = rtw_get_bcmc_stainfo23a(adapter); + else + /* get ap_info */ + *psta = rtw_get_stainfo23a(pstapriv, pattrib->bssid); + + 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 (ieee80211_is_nullfunc(hdr->frame_control)) { + /* 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)) { + ether_addr_copy(pattrib->dst, hdr->addr1); + ether_addr_copy(pattrib->src, hdr->addr2); + ether_addr_copy(pattrib->bssid, hdr->addr3); + ether_addr_copy(pattrib->ra, pattrib->dst); + ether_addr_copy(pattrib->ta, pattrib->src); + + /* */ + ether_addr_copy(pattrib->bssid, mybssid); + + /* get sta_info */ + *psta = rtw_get_stainfo23a(pstapriv, pattrib->bssid); + 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) == true) { + /* Special case */ + ret = RTW_RX_HANDLED; + goto exit; + } else { + if (ether_addr_equal(myhwaddr, pattrib->dst) && !bmcast) { + *psta = rtw_get_stainfo23a(pstapriv, pattrib->bssid); + if (*psta == NULL) { + DBG_8723A("issue_deauth23a to the ap =" MAC_FMT + " for the reason(7)\n", + MAC_ARG(pattrib->bssid)); + + issue_deauth23a(adapter, pattrib->bssid, + WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + } + + ret = _FAIL; + } + +exit: + + + + return ret; +} + +int sta2ap_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame, + struct sta_info **psta); +int sta2ap_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame, + struct sta_info **psta) +{ + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + struct rx_pkt_attrib *pattrib = & precv_frame->attrib; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + 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 (!ether_addr_equal(pattrib->bssid, mybssid)) { + ret = _FAIL; + goto exit; + } + + *psta = rtw_get_stainfo23a(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_8723A("issue_deauth23a to sta =" MAC_FMT + " for the reason(7)\n", + MAC_ARG(pattrib->src)); + + issue_deauth23a(adapter, pattrib->src, + WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + + ret = RTW_RX_HANDLED; + goto exit; + } + + process23a_pwrbit_data(adapter, precv_frame); + + /* We only get here if it's a data frame, so no need to + * confirm data frame type first */ + if (ieee80211_is_data_qos(hdr->frame_control)) + process_wmmps_data(adapter, precv_frame); + + if (ieee80211_is_nullfunc(hdr->frame_control)) { + /* 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 (!ether_addr_equal(pattrib->ra, myhwaddr)) { + ret = RTW_RX_HANDLED; + goto exit; + } + DBG_8723A("issue_deauth23a to sta =" MAC_FMT " for the reason(7)\n", + MAC_ARG(pattrib->src)); + issue_deauth23a(adapter, pattrib->src, + WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + ret = RTW_RX_HANDLED; + goto exit; + } + +exit: + + + + return ret; +} + +int validate_recv_ctrl_frame(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +int validate_recv_ctrl_frame(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ +#ifdef CONFIG_8723AU_AP_MODE + struct rx_pkt_attrib *pattrib = &precv_frame->attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 *pframe = skb->data; + + if (!ieee80211_is_ctl(hdr->frame_control)) + return _FAIL; + + /* receive the frames that ra(a1) is my address */ + if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv))) + return _FAIL; + + /* only handle ps-poll */ + if (ieee80211_is_pspoll(hdr->frame_control)) { + u16 aid; + u8 wmmps_ac = 0; + struct sta_info *psta = NULL; + + aid = GetAid(pframe); + psta = rtw_get_stainfo23a(pstapriv, hdr->addr2); + + if ((!psta) || (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_8723A("%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 & CHKBIT(psta->aid))) { + struct list_head *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe; + 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 (!list_empty(xmitframe_phead)) { + 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; + + /* DBG_8723A("handling ps-poll, q_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */ + + rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe); + + if (psta->sleepq_len == 0) { + pstapriv->tim_bitmap &= ~CHKBIT(psta->aid); + + /* DBG_8723A("after handling ps-poll, tim =%x\n", pstapriv->tim_bitmap); */ + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon23a(padapter, _TIM_IE_, + NULL, false); + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + } else { + /* spin_unlock_bh(&psta->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + /* DBG_8723A("no buffered packets to xmit\n"); */ + if (pstapriv->tim_bitmap & CHKBIT(psta->aid)) { + if (psta->sleepq_len == 0) { + DBG_8723A("no buffered packets " + "to xmit\n"); + + /* issue nulldata with More data bit = 0 to indicate we have no buffered packets */ + issue_nulldata23a(padapter, + psta->hwaddr, + 0, 0, 0); + } else { + DBG_8723A("error!psta->sleepq" + "_len =%d\n", + psta->sleepq_len); + psta->sleepq_len = 0; + } + + pstapriv->tim_bitmap &= ~CHKBIT(psta->aid); + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon23a(padapter, _TIM_IE_, + NULL, false); + } + } + } + } + +#endif + return _FAIL; +} + +struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +int validate_recv_mgnt_frame(struct rtw_adapter *padapter, + struct recv_frame *precv_frame); +int validate_recv_mgnt_frame(struct rtw_adapter *padapter, + struct recv_frame *precv_frame) +{ + struct sta_info *psta; + struct sk_buff *skb; + struct ieee80211_hdr *hdr; + /* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */ + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + ("+validate_recv_mgnt_frame\n")); + + precv_frame = recvframe_chk_defrag23a(padapter, precv_frame); + if (precv_frame == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, + ("%s: fragment packet\n", __func__)); + return _SUCCESS; + } + + skb = precv_frame->pkt; + hdr = (struct ieee80211_hdr *) skb->data; + + /* for rx pkt statistics */ + psta = rtw_get_stainfo23a(&padapter->stapriv, hdr->addr2); + if (psta) { + psta->sta_stats.rx_mgnt_pkts++; + + if (ieee80211_is_beacon(hdr->frame_control)) + psta->sta_stats.rx_beacon_pkts++; + else if (ieee80211_is_probe_req(hdr->frame_control)) + psta->sta_stats.rx_probereq_pkts++; + else if (ieee80211_is_probe_resp(hdr->frame_control)) { + if (ether_addr_equal(padapter->eeprompriv.mac_addr, + hdr->addr1)) + psta->sta_stats.rx_probersp_pkts++; + else if (is_broadcast_ether_addr(hdr->addr1) || + is_multicast_ether_addr(hdr->addr1)) + psta->sta_stats.rx_probersp_bm_pkts++; + else + psta->sta_stats.rx_probersp_uo_pkts++; + } + } + + mgt_dispatcher23a(padapter, precv_frame); + + return _SUCCESS; +} + +int validate_recv_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame); +int validate_recv_data_frame(struct rtw_adapter *adapter, + struct recv_frame *precv_frame) +{ + u8 bretry; + u8 *psa, *pda, *pbssid; + struct sta_info *psta = NULL; + u8 *ptr = precv_frame->pkt->data; + struct rx_pkt_attrib *pattrib = & precv_frame->attrib; + struct security_priv *psecuritypriv = &adapter->securitypriv; + int ret = _SUCCESS; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + + + + bretry = ieee80211_has_retry(hdr->frame_control); + pda = ieee80211_get_DA(hdr); + psa = ieee80211_get_SA(hdr); + pbssid = get_hdr_bssid(ptr); + + if (pbssid == NULL) { + ret = _FAIL; + goto exit; + } + + ether_addr_copy(pattrib->dst, pda); + ether_addr_copy(pattrib->src, psa); + + ether_addr_copy(pattrib->bssid, pbssid); + + switch (pattrib->to_fr_ds) + { + case 0: + ether_addr_copy(pattrib->ra, pda); + ether_addr_copy(pattrib->ta, psa); + ret = sta2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 1: + ether_addr_copy(pattrib->ra, pda); + ether_addr_copy(pattrib->ta, pbssid); + ret = ap2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 2: + ether_addr_copy(pattrib->ra, pbssid); + ether_addr_copy(pattrib->ta, psa); + ret = sta2ap_data_frame(adapter, precv_frame, &psta); + break; + + case 3: + ether_addr_copy(pattrib->ra, hdr->addr1); + ether_addr_copy(pattrib->ta, hdr->addr2); + ret = _FAIL; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n")); + break; + + default: + ret = _FAIL; + break; + } + + if ((ret == _FAIL) || (ret == RTW_RX_HANDLED)) + goto exit; + + if (!psta) { + 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->hdrlen = sizeof(struct ieee80211_hdr_3addr); + if (ieee80211_has_a4(hdr->frame_control)) + pattrib->hdrlen += ETH_ALEN; + + /* parsing QC field */ + if (pattrib->qos == 1) { + __le16 *qptr = (__le16 *)ieee80211_get_qos_ctl(hdr); + u16 qos_ctrl = le16_to_cpu(*qptr); + + pattrib->priority = qos_ctrl & IEEE80211_QOS_CTL_TID_MASK; + pattrib->ack_policy = (qos_ctrl >> 5) & 3; + pattrib->amsdu = + (qos_ctrl & IEEE80211_QOS_CTL_A_MSDU_PRESENT) >> 7; + pattrib->hdrlen += IEEE80211_QOS_CTL_LEN; + + if (pattrib->priority != 0 && pattrib->priority != 3) { + adapter->recvpriv.bIsAnyNonBEPkts = true; + } + } else { + pattrib->priority = 0; + pattrib->ack_policy = 0; + pattrib->amsdu = 0; + } + + 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_multicast_ether_addr" + "(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n", + pattrib->ra[0], + is_multicast_ether_addr(pattrib->ra))); + + GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, + is_multicast_ether_addr(pattrib->ra)); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + ("\n pattrib->encrypt =%d\n", pattrib->encrypt)); + + 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; + break; + case _AES_: + pattrib->iv_len = 8; + pattrib->icv_len = 8; + break; + case _SMS4_: + pattrib->iv_len = 18; + pattrib->icv_len = 16; + break; + default: + pattrib->iv_len = 0; + pattrib->icv_len = 0; + break; + } + } else { + pattrib->encrypt = 0; + pattrib->iv_len = 0; + pattrib->icv_len = 0; + } + +exit: + + + + return ret; +} + +static void dump_rx_pkt(struct sk_buff *skb, u16 type, int level) +{ + int i; + u8 *ptr; + + if ((level == 1) || + ((level == 2) && (type == IEEE80211_FTYPE_MGMT)) || + ((level == 3) && (type == IEEE80211_FTYPE_DATA))) { + + ptr = skb->data; + + DBG_8723A("#############################\n"); + + for (i = 0; i < 64; i = i + 8) + DBG_8723A("%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_8723A("#############################\n"); + } +} + +static int validate_recv_frame(struct rtw_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; + struct rx_pkt_attrib *pattrib = & precv_frame->attrib; + struct sk_buff *skb = precv_frame->pkt; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + u8 ver; + u8 bDumpRxPkt; + u16 seq_ctrl, fctl; + + fctl = le16_to_cpu(hdr->frame_control); + ver = fctl & IEEE80211_FCTL_VERS; + type = fctl & IEEE80211_FCTL_FTYPE; + subtype = fctl & IEEE80211_FCTL_STYPE; + + /* 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; + } + + pattrib->to_fr_ds = get_tofr_ds(hdr->frame_control); + + seq_ctrl = le16_to_cpu(hdr->seq_ctrl); + pattrib->frag_num = seq_ctrl & IEEE80211_SCTL_FRAG; + pattrib->seq_num = seq_ctrl >> 4; + + pattrib->pw_save = ieee80211_has_pm(hdr->frame_control); + pattrib->mfrag = ieee80211_has_morefrags(hdr->frame_control); + pattrib->mdata = ieee80211_has_moredata(hdr->frame_control); + pattrib->privacy = ieee80211_has_protected(hdr->frame_control); + pattrib->order = ieee80211_has_order(hdr->frame_control); + + rtw_hal_get_def_var23a(adapter, HAL_DEF_DBG_DUMP_RXPKT, &bDumpRxPkt); + + if (unlikely(bDumpRxPkt == 1)) + dump_rx_pkt(skb, type, bDumpRxPkt); + + switch (type) + { + case IEEE80211_FTYPE_MGMT: + 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 IEEE80211_FTYPE_CTL: + 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 IEEE80211_FTYPE_DATA: + rtw_led_control(adapter, LED_CTL_RX); + pattrib->qos = (subtype & IEEE80211_STYPE_QOS_DATA) ? 1 : 0; + retval = validate_recv_data_frame(adapter, precv_frame); + if (retval == _FAIL) { + struct recv_priv *precvpriv = &adapter->recvpriv; + /* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail\n")); */ + 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) +{ + u16 eth_type, len, hdrlen; + u8 bsnaphdr; + u8 *psnap; + + int ret = _SUCCESS; + struct rtw_adapter *adapter = precvframe->adapter; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + struct sk_buff *skb = precvframe->pkt; + u8 *ptr; + struct rx_pkt_attrib *pattrib = &precvframe->attrib; + + + + ptr = skb->data; + hdrlen = pattrib->hdrlen; + psnap = ptr + hdrlen; + eth_type = (psnap[6] << 8) | psnap[7]; + /* convert hdr + possible LLC headers into Ethernet header */ + /* eth_type = (psnap_type[0] << 8) | psnap_type[1]; */ + if ((ether_addr_equal(psnap, rfc1042_header) && + eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || + ether_addr_equal(psnap, bridge_tunnel_header)) { + /* remove RFC1042 or Bridge-Tunnel encapsulation + and replace EtherType */ + bsnaphdr = true; + hdrlen += SNAP_SIZE; + } else { + /* Leave Ethernet header part of hdr and full payload */ + bsnaphdr = false; + eth_type = (psnap[0] << 8) | psnap[1]; + } + + len = skb->len - hdrlen; + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + ("\n === pattrib->hdrlen: %x, pattrib->iv_len:%x ===\n\n", + pattrib->hdrlen, pattrib->iv_len)); + + pattrib->eth_type = eth_type; + if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) { + ptr += hdrlen; + *ptr = 0x87; + *(ptr + 1) = 0x12; + + eth_type = 0x8712; + /* append rx status for mp test packets */ + + ptr = skb_pull(skb, (hdrlen - sizeof(struct ethhdr) + 2) - 24); + memcpy(ptr, skb->head, 24); + ptr += 24; + } else { + ptr = skb_pull(skb, (hdrlen - sizeof(struct ethhdr) + + (bsnaphdr ? 2:0))); + } + + ether_addr_copy(ptr, pattrib->dst); + ether_addr_copy(ptr + ETH_ALEN, pattrib->src); + + if (!bsnaphdr) { + len = htons(len); + memcpy(ptr + 12, &len, 2); + } + + + return ret; +} + +/* perform defrag */ +struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter, + struct rtw_queue *defrag_q); +struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter, + struct rtw_queue *defrag_q) +{ + struct list_head *plist, *phead, *ptmp; + u8 *data, wlanhdr_offset; + u8 curfragnum; + struct recv_frame *pnfhdr; + struct recv_frame *prframe, *pnextrframe; + struct rtw_queue *pfree_recv_queue; + struct sk_buff *skb; + + + + curfragnum = 0; + pfree_recv_queue = &adapter->recvpriv.free_recv_queue; + + phead = get_list_head(defrag_q); + plist = phead->next; + prframe = container_of(plist, struct recv_frame, list); + list_del_init(&prframe->list); + skb = prframe->pkt; + + if (curfragnum != prframe->attrib.frag_num) { + /* the first fragment number must be 0 */ + /* free the whole queue */ + rtw_free_recvframe23a(prframe, pfree_recv_queue); + rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue); + + return NULL; + } + + curfragnum++; + + phead = get_list_head(defrag_q); + + data = prframe->pkt->data; + + list_for_each_safe(plist, ptmp, phead) { + 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_recvframe23a(prframe, pfree_recv_queue); + rtw_free_recvframe23a_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; + + skb_pull(pnfhdr->pkt, wlanhdr_offset); + + /* append to first fragment frame's tail + (if privacy frame, pull the ICV) */ + + skb_trim(skb, skb->len - prframe->attrib.icv_len); + + memcpy(skb_tail_pointer(skb), pnfhdr->pkt->data, + pnfhdr->pkt->len); + + skb_put(skb, pnfhdr->pkt->len); + + prframe->attrib.icv_len = pnfhdr->attrib.icv_len; + }; + + /* free the defrag_q queue and return the prframe */ + rtw_free_recvframe23a_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_defrag23a(struct rtw_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 rtw_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_stainfo23a(pstapriv, psta_addr); + if (!psta) { + struct ieee80211_hdr *hdr = + (struct ieee80211_hdr *) pfhdr->pkt->data; + if (!ieee80211_is_data(hdr->frame_control)) { + psta = rtw_get_bcmc_stainfo23a(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 (_rtw_queue_empty23a(pdefrag_q) == false) { + /* free current defrag_q */ + rtw_free_recvframe23a_queue(pdefrag_q, pfree_recv_queue); + } + } + + /* Then enqueue the 0~(n-1) fragment into the + defrag_q */ + + /* spin_lock(&pdefrag_q->lock); */ + phead = get_list_head(pdefrag_q); + list_add_tail(&pfhdr->list, phead); + /* spin_unlock(&pdefrag_q->lock); */ + + 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 */ + rtw_free_recvframe23a(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) { + /* spin_lock(&pdefrag_q->lock); */ + phead = get_list_head(pdefrag_q); + list_add_tail(&pfhdr->list, phead); + /* spin_unlock(&pdefrag_q->lock); */ + + /* 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 */ + rtw_free_recvframe23a(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")); + rtw_free_recvframe23a(prtnframe, pfree_recv_queue); + prtnframe = NULL; + } + } + + + + return prtnframe; +} + +int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe); +int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib; + struct sk_buff *skb, *sub_skb; + struct sk_buff_head skb_list; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct rtw_queue *pfree_recv_queue = &precvpriv->free_recv_queue; + + pattrib = &prframe->attrib; + + skb = prframe->pkt; + skb_pull(skb, prframe->attrib.hdrlen); + __skb_queue_head_init(&skb_list); + + ieee80211_amsdu_to_8023s(skb, &skb_list, NULL, 0, 0, false); + + while (!skb_queue_empty(&skb_list)) { + sub_skb = __skb_dequeue(&skb_list); + + 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); + } + + prframe->pkt = NULL; + rtw_free_recvframe23a(prframe, pfree_recv_queue); + return _SUCCESS; +} + +int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num); +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; + + /* 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)) { + /* boundary situation, when seq_num cross 0xFFF */ + 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_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl, + struct recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib = &prframe->attrib; + struct rtw_queue *ppending_recvframe_queue; + struct list_head *phead, *plist, *ptmp; + struct recv_frame *hdr; + struct rx_pkt_attrib *pnextattrib; + + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + /* DbgPrint("+enqueue_reorder_recvframe23a()\n"); */ + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock); */ + /* spin_lock_ex(&ppending_recvframe_queue->lock); */ + + phead = get_list_head(ppending_recvframe_queue); + + list_for_each_safe(plist, ptmp, phead) { + hdr = container_of(plist, struct recv_frame, list); + pnextattrib = &hdr->attrib; + + if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num)) { + continue; + } else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num)) { + /* Duplicate entry is found!! Do not insert current entry. */ + /* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */ + + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */ + return false; + } else { + break; + } + + /* DbgPrint("enqueue_reorder_recvframe23a():while\n"); */ + } + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock); */ + /* spin_lock_ex(&ppending_recvframe_queue->lock); */ + + list_del_init(&prframe->list); + + list_add_tail(&prframe->list, plist); + + /* spin_unlock_ex(&ppending_recvframe_queue->lock); */ + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */ + + /* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */ + return true; +} + +int recv_indicatepkts_in_order(struct rtw_adapter *padapter, + struct recv_reorder_ctrl *preorder_ctrl, + int bforced); +int recv_indicatepkts_in_order(struct rtw_adapter *padapter, + struct recv_reorder_ctrl *preorder_ctrl, + int bforced) +{ + /* u8 bcancelled; */ + struct list_head *phead, *plist; + struct recv_frame *prframe; + struct rx_pkt_attrib *pattrib; + /* u8 index = 0; */ + int bPktInBuf = false; + struct recv_priv *precvpriv; + struct rtw_queue *ppending_recvframe_queue; + + precvpriv = &padapter->recvpriv; + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + /* DbgPrint("+recv_indicatepkts_in_order\n"); */ + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock); */ + /* spin_lock_ex(&ppending_recvframe_queue->lock); */ + + phead = get_list_head(ppending_recvframe_queue); + plist = phead->next; + + /* Handling some condition for forced indicate case. */ + if (bforced) { + if (list_empty(phead)) { + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */ + /* spin_unlock_ex(&ppending_recvframe_queue->lock); */ + 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; + } + + if (!pattrib->amsdu) { + if ((padapter->bDriverStopped == false) && + (padapter->bSurpriseRemoved == false)) { + rtw_recv_indicatepkt23a(padapter, prframe); + } + } else { + if (amsdu_to_msdu(padapter, prframe) != + _SUCCESS) { + rtw_free_recvframe23a(prframe, + &precvpriv->free_recv_queue); + } + } + + /* Update local variables. */ + bPktInBuf = false; + + } else { + bPktInBuf = true; + break; + } + + /* DbgPrint("recv_indicatepkts_in_order():while\n"); */ + } + + /* spin_unlock_ex(&ppending_recvframe_queue->lock); */ + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock); */ + + return bPktInBuf; +} + +int recv_indicatepkt_reorder(struct rtw_adapter *padapter, + struct recv_frame *prframe); +int recv_indicatepkt_reorder(struct rtw_adapter *padapter, + struct recv_frame *prframe) +{ + int retval = _SUCCESS; + struct rx_pkt_attrib *pattrib; + struct recv_reorder_ctrl *preorder_ctrl; + struct rtw_queue *ppending_recvframe_queue; + + pattrib = &prframe->attrib; + preorder_ctrl = prframe->preorder_ctrl; + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + if (!pattrib->amsdu) { + /* s1. */ + wlanhdr_to_ethhdr(prframe); + + if ((pattrib->qos!= 1) || (pattrib->eth_type == 0x0806) || + (pattrib->ack_policy != 0)) { + if ((padapter->bDriverStopped == false) && + (padapter->bSurpriseRemoved == false)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, + ("@@@@ recv_indicatepkt_reorder -" + "recv_func recv_indicatepkt\n")); + + rtw_recv_indicatepkt23a(padapter, prframe); + return _SUCCESS; + } + + return _FAIL; + } + + if (preorder_ctrl->enable == false) { + /* indicate this recv_frame */ + preorder_ctrl->indicate_seq = pattrib->seq_num; + rtw_recv_indicatepkt23a(padapter, prframe); + + preorder_ctrl->indicate_seq = + (preorder_ctrl->indicate_seq + 1) % 4096; + return _SUCCESS; + } + } else { + /* temp filter -> means didn't support A-MSDUs in a A-MPDU */ + if (preorder_ctrl->enable == false) { + 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_recvframe23a(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. */ + /* */ + + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false) == true) { + mod_timer(&preorder_ctrl->reordering_ctrl_timer, + jiffies + msecs_to_jiffies(REORDER_WAIT_TIME)); + spin_unlock_bh(&ppending_recvframe_queue->lock); + } else { + spin_unlock_bh(&ppending_recvframe_queue->lock); + del_timer_sync(&preorder_ctrl->reordering_ctrl_timer); + } + return _SUCCESS; + +_err_exit: + + spin_unlock_bh(&ppending_recvframe_queue->lock); + return _FAIL; +} + +void rtw_reordering_ctrl_timeout_handler23a(unsigned long pcontext) +{ + struct recv_reorder_ctrl *preorder_ctrl; + struct rtw_adapter *padapter; + struct rtw_queue *ppending_recvframe_queue; + + preorder_ctrl = (struct recv_reorder_ctrl *)pcontext; + padapter = preorder_ctrl->padapter; + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) { + return; + } + + /* DBG_8723A("+rtw_reordering_ctrl_timeout_handler23a() =>\n"); */ + + spin_lock_bh(&ppending_recvframe_queue->lock); + + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true) { + mod_timer(&preorder_ctrl->reordering_ctrl_timer, + jiffies + msecs_to_jiffies(REORDER_WAIT_TIME)); + } + + spin_unlock_bh(&ppending_recvframe_queue->lock); +} + +int process_recv_indicatepkts(struct rtw_adapter *padapter, + struct recv_frame *prframe); +int process_recv_indicatepkts(struct rtw_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 == true) { /* B/G/N Mode */ + /* prframe->preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */ + + /* including perform A-MPDU Rx Ordering Buffer Control */ + if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) { + if ((padapter->bDriverStopped == false) && + (padapter->bSurpriseRemoved == false)) { + 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 == false) && + (padapter->bSurpriseRemoved == false)) { + /* indicate this recv_frame */ + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, + ("@@@@ process_recv_indicatepkts- " + "recv_func recv_indicatepkt\n")); + rtw_recv_indicatepkt23a(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 rtw_adapter *padapter, + struct recv_frame *rframe) +{ + struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + int ret = _SUCCESS; + + /* 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_recvframe23a(rframe, pfree_recv_queue); + goto exit; + } + +exit: + return ret; +} + +static int recv_func_posthandle(struct rtw_adapter *padapter, + struct recv_frame *prframe) +{ + int ret = _SUCCESS; + struct recv_frame *orig_prframe = prframe; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct rtw_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_defrag23a(padapter, prframe); + if (!prframe) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, + ("recvframe_chk_defrag23a: drop pkt\n")); + goto _recv_data_drop; + } + + /* + * Pull off crypto headers + */ + if (prframe->attrib.iv_len > 0) { + skb_pull(prframe->pkt, prframe->attrib.iv_len); + } + + if (prframe->attrib.icv_len > 0) { + skb_trim(prframe->pkt, + prframe->pkt->len - prframe->attrib.icv_len); + } + + prframe = portctrl(padapter, prframe); + if (!prframe) { + 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_recvframe23a(orig_prframe, pfree_recv_queue);/* free this recv_frame */ + goto _recv_data_drop; + } + return ret; + +_recv_data_drop: + precvpriv->rx_drop++; + return ret; +} + +int rtw_recv_entry23a(struct recv_frame *rframe) +{ + int ret, r; + struct rtw_adapter *padapter = rframe->adapter; + struct rx_pkt_attrib *prxattrib = &rframe->attrib; + struct recv_priv *recvpriv = &padapter->recvpriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + + /* check if need to handle uc_swdec_pending_queue*/ + if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && + psecuritypriv->busetkipkey) { + struct recv_frame *pending_frame; + + while ((pending_frame = rtw_alloc_recvframe23a(&padapter->recvpriv.uc_swdec_pending_queue))) { + r = recv_func_posthandle(padapter, pending_frame); + if (r == _SUCCESS) + DBG_8723A("%s: dequeue uc_swdec_pending_queue\n", __func__); + } + } + + 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_multicast_ether_addr(prxattrib->ra) && + prxattrib->encrypt > 0 && + (prxattrib->bdecrypted == 0) && + !is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm) && + !psecuritypriv->busetkipkey) { + rtw_enqueue_recvframe23a(rframe, &padapter->recvpriv.uc_swdec_pending_queue); + DBG_8723A("%s: no key, enqueue uc_swdec_pending_queue\n", __func__); + goto exit; + } + + ret = recv_func_posthandle(padapter, rframe); + + recvpriv->rx_pkts++; + } + +exit: + return ret; +} + +void rtw_signal_stat_timer_hdl23a(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct recv_priv *recvpriv = &adapter->recvpriv; + + u32 tmp_s, tmp_q; + u8 avg_signal_strength = 0; + u8 avg_signal_qual = 0; + u32 num_signal_strength = 0; + u32 num_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; + num_signal_strength = + recvpriv->signal_strength_data.total_num; + /* 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; + num_signal_qual = recvpriv->signal_qual_data.total_num; + /* 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; + + DBG_8723A("%s signal_strength:%3u, rssi:%3d, " + "signal_qual:%3u, num_signal_strength:%u, " + "num_signal_qual:%u\n", + __func__, recvpriv->signal_strength, + recvpriv->rssi, recvpriv->signal_qual, + num_signal_strength, num_signal_qual + ); + } + } + rtw_set_signal_stat_timer(recvpriv); +} diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c new file mode 100644 index 000000000000..0d457144fde7 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_security.c @@ -0,0 +1,1653 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_SECURITY_C_ + +#include +#include +#include +#include + +/* 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 = 0; +static u32 crc32_table[256]; + +static u8 crc32_reverseBit(u8 data) +{ + u8 retval = ((data << 7) & 0x80) | ((data << 5) & 0x40) | + ((data << 3) & 0x20) | ((data << 1) & 0x10) | + ((data >> 1) & 0x08) | ((data >> 3) & 0x04) | + ((data >> 5) & 0x02) | ((data >> 7) & 0x01); + return retval; +} + +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 u32 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 ~crc; /* transmit complement, per CRC-32 spec */ +} + +/* Need to consider the fragment situation */ +void rtw_wep_encrypt23a(struct rtw_adapter *padapter, + struct xmit_frame *pxmitframe) +{ + /* exclude ICV */ + unsigned char crc[4]; + struct arc4context mycontext; + int curfragnum, length, index; + u32 keylength; + u8 *pframe, *payload, *iv; /* wepkey */ + u8 wepkey[16]; + u8 hw_hdr_offset = 0; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (!pxmitframe->buf_addr) + return; + + hw_hdr_offset = TXDESC_OFFSET; + + pframe = pxmitframe->buf_addr + hw_hdr_offset; + + /* start to encrypt each fragment */ + if ((pattrib->encrypt != _WEP40_) && (pattrib->encrypt != _WEP104_)) + return; + + index = psecuritypriv->dot11PrivacyKeyIndex; + keylength = psecuritypriv->dot11DefKeylen[index]; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags ; curfragnum++) { + iv = pframe + pattrib->hdrlen; + memcpy(&wepkey[0], iv, 3); + memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[index].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; + + *((u32 *)crc) = cpu_to_le32(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; + *((u32 *)crc) = cpu_to_le32(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((unsigned long)(pframe)); + } + } + +} + +void rtw_wep_decrypt23a(struct rtw_adapter *padapter, + struct recv_frame *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 = &precvframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct sk_buff * skb = precvframe->pkt; + + pframe = skb->data; + + /* start to decrypt recvframe */ + if ((prxattrib->encrypt =! _WEP40_) && (prxattrib->encrypt != _WEP104_)) + return; + + iv = pframe + prxattrib->hdrlen; + /* keyindex = (iv[3]&0x3); */ + keyindex = prxattrib->key_index; + keylength = psecuritypriv->dot11DefKeylen[keyindex]; + memcpy(&wepkey[0], iv, 3); + /* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */ + memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], + keylength); + length = skb->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 */ + *((u32 *)crc) = le32_to_cpu(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_decrypt23a:icv error crc[3](%x)!= payload" + "[length-1](%x) || crc[2](%x)!= payload[length-2](%x)" + " || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)" + "!= payload[length-4](%x)\n", + crc[3], payload[length - 1], + crc[2], payload[length - 2], + crc[1], payload[length - 3], + crc[0], payload[length - 4])); + } + + return; +} + +/* 3 ===== TKIP related ===== */ + +static u32 secmicgetuint32(u8 * p) +/* Convert from Byte[] to u32 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 long 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_secmicsetkey23a(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_secmicappend23abyte23a(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_secmicappend23a(struct mic_data *pmicdata, u8 * src, u32 nbytes) +{ + + /* This is simple */ + while(nbytes > 0) + { + rtw_secmicappend23abyte23a(pmicdata, *src++); + nbytes--; + } + +} + +void rtw_secgetmic23a(struct mic_data *pmicdata, u8 * dst) +{ + + /* Append the minimum padding */ + rtw_secmicappend23abyte23a(pmicdata, 0x5a); + rtw_secmicappend23abyte23a(pmicdata, 0); + rtw_secmicappend23abyte23a(pmicdata, 0); + rtw_secmicappend23abyte23a(pmicdata, 0); + rtw_secmicappend23abyte23a(pmicdata, 0); + /* and then zeroes until the length is a multiple of 4 */ + while(pmicdata->nBytesInM != 0) + { + rtw_secmicappend23abyte23a(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_seccalctkipmic23a(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_secmicsetkey23a(&micdata, key); + priority[0]= pri; + + /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */ + if (header[1]&1) { /* ToDS == 1 */ + rtw_secmicappend23a(&micdata, &header[16], 6); /* DA */ + if (header[1]&2) /* From Ds == 1 */ + rtw_secmicappend23a(&micdata, &header[24], 6); + else + rtw_secmicappend23a(&micdata, &header[10], 6); + } + else{ /* ToDS == 0 */ + rtw_secmicappend23a(&micdata, &header[4], 6); /* DA */ + if (header[1]&2) /* From Ds == 1 */ + rtw_secmicappend23a(&micdata, &header[16], 6); + else + rtw_secmicappend23a(&micdata, &header[10], 6); + + } + rtw_secmicappend23a(&micdata, &priority[0], 4); + + rtw_secmicappend23a(&micdata, data, data_len); + + rtw_secgetmic23a(&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_encrypt23a(struct rtw_adapter *padapter, + struct xmit_frame *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; + u32 prwskeylen; + + u8 *pframe, *payload,*iv,*prwskey; + union pn48 dot11txpn; + struct sta_info *stainfo; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u32 res = _SUCCESS; + + if (!pxmitframe->buf_addr) + return _FAIL; + + hw_hdr_offset = TXDESC_OFFSET; + + pframe = pxmitframe->buf_addr + hw_hdr_offset; + /* 4 start to encrypt each fragment */ + if (pattrib->encrypt == _TKIP_) { + + if (pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + DBG_8723A("%s, call rtw_get_stainfo()\n", __func__); + stainfo = rtw_get_stainfo23a(&padapter->stapriv, + &pattrib->ra[0]); + } + + if (stainfo!= NULL) { + + if (!(stainfo->state &_FW_LINKED)) + { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state); + return _FAIL; + } + + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo!= NULL!!!\n")); + + if (is_multicast_ether_addr(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + + prwskeylen = 16; + + for (curfragnum = 0;curfragnumnr_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)); + *((u32 *)crc) = cpu_to_le32(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 ; + *((u32 *)crc) = cpu_to_le32(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((unsigned long)(pframe)); + + } + } + + } + else{ + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo == NULL!!!\n")); + DBG_8723A("%s, psta == NUL\n", __func__); + res = _FAIL; + } + + } + + return res; +} + +/* The hlen isn't include the IV */ +u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter, + struct recv_frame *precvframe) +{ + /* exclude ICV */ + u16 pnl; + u32 pnh; + u8 rc4key[16]; + u8 ttkey[16]; + u8 crc[4]; + struct arc4context mycontext; + int length; + u32 prwskeylen; + u8 *pframe, *payload,*iv,*prwskey; + union pn48 dot11txpn; + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &precvframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct sk_buff * skb = precvframe->pkt; + u32 res = _SUCCESS; + + pframe = skb->data; + + /* 4 start to decrypt recvframe */ + if (prxattrib->encrypt == _TKIP_) { + + stainfo = rtw_get_stainfo23a(&padapter->stapriv, + &prxattrib->ta[0]); + if (stainfo!= NULL) { + + if (is_multicast_ether_addr(prxattrib->ra)) { + if (psecuritypriv->binstallGrpkey == false) { + res = _FAIL; + DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__); + goto exit; + } + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + prwskeylen = 16; + } else { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: stainfo!= NULL!!!\n")); + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + prwskeylen = 16; + } + + iv = pframe+prxattrib->hdrlen; + payload = pframe+prxattrib->iv_len+prxattrib->hdrlen; + length = skb->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); + + *((u32 *)crc) = le32_to_cpu(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_decrypt23a:icv error crc[3](%x)!= payload[length-1](%x) || crc[2](%x)!= payload[length-2](%x) || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)!= payload[length-4](%x)\n", + crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4])); + res = _FAIL; + } + } else { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: 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 construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, + int qc_exists); + +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 halfs */ + 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 = 0; + + 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]; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + u16 frsubtype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE; + + 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 == sizeof(struct ieee80211_hdr_3addr) || + (hdrlen == sizeof(struct ieee80211_qos_hdr)))) + a4_exists = 0; + else + a4_exists = 1; + + if (ieee80211_is_data(hdr->frame_control)) { + if ((frsubtype == IEEE80211_STYPE_DATA_CFACK) || + (frsubtype == IEEE80211_STYPE_DATA_CFPOLL) || + (frsubtype == IEEE80211_STYPE_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != sizeof(struct ieee80211_qos_hdr)) + hdrlen += 2; + } else if ((frsubtype == IEEE80211_STYPE_QOS_DATA) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACK) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFPOLL) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACKPOLL)) { + if (hdrlen != sizeof(struct ieee80211_qos_hdr)) + hdrlen += 2; + qc_exists = 1; + } else { + qc_exists = 0; + } + } 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); + + 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++]; + 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]; + + 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_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe) +{ /* exclude ICV */ + /* Intermediate Buffers */ + int curfragnum, length; + u32 prwskeylen; + u8 *pframe, *prwskey; /* *payload,*iv */ + u8 hw_hdr_offset = 0; + struct sta_info *stainfo; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u32 res = _SUCCESS; + + if (!pxmitframe->buf_addr) + return _FAIL; + + hw_hdr_offset = TXDESC_OFFSET; + + pframe = pxmitframe->buf_addr + hw_hdr_offset; + + /* 4 start to encrypt each fragment */ + if (pattrib->encrypt != _AES_) + return _FAIL; + + if (pattrib->psta) { + stainfo = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + stainfo = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]); + } + + if (!stainfo) { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, + ("rtw_aes_encrypt23a: stainfo == NULL!!!\n")); + DBG_8723A("%s, psta == NUL\n", __func__); + res = _FAIL; + goto out; + } + if (!(stainfo->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", + __func__, stainfo->state); + return _FAIL; + } + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, + ("rtw_aes_encrypt23a: stainfo!= NULL!!!\n")); + + if (is_multicast_ether_addr(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + + prwskeylen = 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + /* 4 the last fragment */ + if ((curfragnum + 1) == pattrib->nr_frags) { + 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((unsigned long)pframe); + } + } +out: + 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]; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + u16 frsubtype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE; + + 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 == sizeof(struct ieee80211_hdr_3addr) || + (hdrlen == sizeof(struct ieee80211_qos_hdr)))) + a4_exists = 0; + else + a4_exists = 1; + + if (ieee80211_is_data(hdr->frame_control)) { + if ((frsubtype == IEEE80211_STYPE_DATA_CFACK) || + (frsubtype == IEEE80211_STYPE_DATA_CFPOLL) || + (frsubtype == IEEE80211_STYPE_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != sizeof(struct ieee80211_hdr_3addr)) + hdrlen += 2; + } else if ((frsubtype == IEEE80211_STYPE_QOS_DATA) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACK) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFPOLL) || + (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACKPOLL)) { + if (hdrlen != sizeof(struct ieee80211_hdr_3addr)) + hdrlen += 2; + qc_exists = 1; + } else { + qc_exists = 0; + } + } 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_8723A("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_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvframe) +{ /* exclude ICV */ + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &precvframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct sk_buff *skb = precvframe->pkt; + int length; + u8 *pframe, *prwskey; /* *payload,*iv */ + u32 res = _SUCCESS; + + pframe = skb->data; + /* 4 start to encrypt each fragment */ + if (!prxattrib->encrypt != _AES_) + return _FAIL; + + stainfo = rtw_get_stainfo23a(&padapter->stapriv, &prxattrib->ta[0]); + if (!stainfo) { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, + ("rtw_aes_encrypt23a: stainfo == NULL!!!\n")); + res = _FAIL; + goto exit; + } + + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, + ("rtw_aes_decrypt23a: stainfo!= NULL!!!\n")); + + if (is_multicast_ether_addr(prxattrib->ra)) { + /* in concurrent we should use sw decrypt in group key, + so we remove this message */ + if (!psecuritypriv->binstallGrpkey) { + res = _FAIL; + DBG_8723A("%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_8723A("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 = skb->len - prxattrib->hdrlen - prxattrib->iv_len; + + res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length); +exit: + return res; +} + +void rtw_use_tkipkey_handler23a(void *FunctionContext) +{ + struct rtw_adapter *padapter = (struct rtw_adapter *)FunctionContext; + + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler23a ^^^\n")); + padapter->securitypriv.busetkipkey = true; + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, + ("^^^rtw_use_tkipkey_handler23a padapter->securitypriv.busetkipkey =%d^^^\n", + padapter->securitypriv.busetkipkey)); +} diff --git a/drivers/staging/rtl8723au/core/rtw_sreset.c b/drivers/staging/rtl8723au/core/rtw_sreset.c new file mode 100644 index 000000000000..8d1a6fe168ac --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_sreset.c @@ -0,0 +1,253 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ + +#include + +void sreset_init_value23a(struct rtw_adapter *padapter) +{ + struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + + 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_value23a(struct rtw_adapter *padapter) +{ + struct hal_data_8723a *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_status23a(struct rtw_adapter *padapter) +{ + struct hal_data_8723a *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_8723A("txdmastatu(%x)\n", val32); + psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR; + } + + if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) { + DBG_8723A("==>%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_8723A("==> %s wifi_status(0x%x)\n", __func__, status); + + /* status restore */ + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + + return status; +} + +void sreset_set_wifi_error_status23a(struct rtw_adapter *padapter, u32 status) +{ + struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter); + + pHalData->srestpriv.Wifi_Error_Status = status; +} + +void sreset_set_trigger_point(struct rtw_adapter *padapter, s32 tgp) +{ + struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter); + + pHalData->srestpriv.dbg_trigger_point = tgp; +} + +bool sreset_inprogress(struct rtw_adapter *padapter) +{ + struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter); + + return pHalData->srestpriv.silent_reset_inprogress; +} + +static void sreset_restore_security_station(struct rtw_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + struct mlme_ext_info *pmlmeinfo = &padapter->mlmeextpriv.mlmext_info; + u8 val8; + + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) + val8 = 0xcc; + else + val8 = 0xcf; + rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + psta = rtw_get_stainfo23a(pstapriv, get_bssid(mlmepriv)); + if (psta == NULL) { + /* DEBUG_ERR(("Set wpa_set_encryption: Obtain Sta_info fail\n")); */ + } else { + /* pairwise key */ + rtw_setstakey_cmd23a(padapter, (unsigned char *)psta, true); + /* group key */ + rtw_set_key23a(padapter,&padapter->securitypriv, padapter->securitypriv.dot118021XGrpKeyid, 0); + } + } +} + +static void sreset_restore_network_station(struct rtw_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + u8 threshold; + + rtw_setopmode_cmd23a(padapter, Ndis802_11Infrastructure); + + /* TH = 1 => means that invalidate usb rx aggregation */ + /* TH = 0 => means that validate usb rx aggregation, use init value. */ + if (mlmepriv->htpriv.ht_option) { + if (padapter->registrypriv.wifi_spec == 1) + threshold = 1; + else + threshold = 0; + rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } else { + threshold = 1; + rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + /* disable dynamic functions, such as high power, DIG */ + /* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */ + + rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress); + + { + u8 join_type = 0; + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + } + + Set_MSR23a(padapter, (pmlmeinfo->state & 0x3)); + + mlmeext_joinbss_event_callback23a(padapter, 1); + /* restore Sequence No. */ + rtw_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn); + + sreset_restore_security_station(padapter); +} + +static void sreset_restore_network_status(struct rtw_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + + if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) { + DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + sreset_restore_network_station(padapter); + } else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) { + DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + rtw_ap_restore_network(padapter); + } else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE)) { + DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + } else { + DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + } +} + +static void sreset_stop_adapter(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter == NULL) + return; + + DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + if (!rtw_netif_queue_stopped(padapter->pnetdev)) + netif_tx_stop_all_queues(padapter->pnetdev); + + rtw_cancel_all_timer23a(padapter); + + /* TODO: OS and HCI independent */ + tasklet_kill(&pxmitpriv->xmit_tasklet); + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) + rtw_scan_abort23a(padapter); + + if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) + rtw23a_join_to_handler((unsigned long)padapter); +} + +static void sreset_start_adapter(struct rtw_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter == NULL) + return; + + DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + if (check_fwstate(pmlmepriv, _FW_LINKED)) { + sreset_restore_network_status(padapter); + } + + /* TODO: OS and HCI independent */ + tasklet_hi_schedule(&pxmitpriv->xmit_tasklet); + + mod_timer(&padapter->mlmepriv.dynamic_chk_timer, + jiffies + msecs_to_jiffies(2000)); + + if (rtw_netif_queue_stopped(padapter->pnetdev)) + netif_tx_wake_all_queues(padapter->pnetdev); +} + +void sreset_reset(struct rtw_adapter *padapter) +{ + struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + unsigned long start = jiffies; + + DBG_8723A("%s\n", __func__); + + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + + mutex_lock(&psrtpriv->silentreset_mutex); + psrtpriv->silent_reset_inprogress = true; + pwrpriv->change_rfpwrstate = rf_off; + + sreset_stop_adapter(padapter); + + ips_enter23a(padapter); + ips_leave23a(padapter); + + sreset_start_adapter(padapter); + psrtpriv->silent_reset_inprogress = false; + mutex_unlock(&psrtpriv->silentreset_mutex); + + DBG_8723A("%s done in %d ms\n", __func__, + jiffies_to_msecs(jiffies - start)); +} diff --git a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c new file mode 100644 index 000000000000..451b58f47287 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c @@ -0,0 +1,509 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_STA_MGT_C_ + +#include +#include +#include +#include +#include +#include + +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_queue23a(&psta->sleep_q); + psta->sleepq_len = 0; + _rtw_init_sta_xmit_priv23a(&psta->sta_xmitpriv); + _rtw_init_sta_recv_priv23a(&psta->sta_recvpriv); +#ifdef CONFIG_8723AU_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; + 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; + psta->keep_alive_trycnt = 0; +#endif /* CONFIG_8723AU_AP_MODE */ +} + +u32 _rtw_init_sta_priv23a(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 - + ((unsigned long)(pstapriv->pallocated_stainfo_buf) & 3); + _rtw_init_queue23a(&pstapriv->free_sta_queue); + spin_lock_init(&pstapriv->sta_hash_lock); + pstapriv->asoc_sta_count = 0; + _rtw_init_queue23a(&pstapriv->sleep_q); + _rtw_init_queue23a(&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_8723AU_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 = 900; 900*2 = 1800 sec = 30 min, expire after no any traffic. */ + /* pstapriv->expire_to = 30; 30*2 = 60 sec = 1 min, expire after no any traffic. */ + pstapriv->expire_to = 3; /* 3*2 = 6 sec */ + pstapriv->max_num_sta = NUM_STA; +#endif + return _SUCCESS; +} + +inline int rtw_stainfo_offset23a(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_8723A("%s invalid offset(%d), out of range!!!", __func__, offset); + return offset; +} + +inline struct sta_info *rtw_get_stainfo23a_by_offset23a(struct sta_priv *stapriv, int offset) +{ + if (!stainfo_offset_valid(offset)) + DBG_8723A("%s invalid offset(%d), out of range!!!", __func__, offset); + return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info)); +} + +/* this function is used to free the memory of lock || sema for all stainfos */ +void rtw_mfree_all_stainfo(struct sta_priv *pstapriv) +{ + struct list_head *plist, *phead; + struct sta_info *psta; + + spin_lock_bh(&pstapriv->sta_hash_lock); + + phead = get_list_head(&pstapriv->free_sta_queue); + + /* we really achieve a lot in this loop .... */ + list_for_each(plist, phead) + psta = container_of(plist, struct sta_info, list); + spin_unlock_bh(&pstapriv->sta_hash_lock); +} + +void rtw_mfree_sta_priv_lock(struct sta_priv *pstapriv) +{ + rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */ +} + +u32 _rtw_free_sta_priv23a(struct sta_priv *pstapriv) +{ + struct list_head *phead, *plist, *ptmp; + struct sta_info *psta; + 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]; + + list_for_each_safe(plist, ptmp, phead) { + int i; + psta = container_of(plist, struct sta_info, + hash_list); + for (i = 0; i < 16 ; i++) { + preorder_ctrl = &psta->recvreorder_ctrl[i]; + del_timer_sync(&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_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr) +{ + struct list_head *phash_list; + struct sta_info *psta; + struct rtw_queue *pfree_sta_queue; + struct recv_reorder_ctrl *preorder_ctrl; + uint tmp_aid; + s32 index; + int i = 0; + u16 wRxSeqInitialValue = 0xffff; + + pfree_sta_queue = &pstapriv->free_sta_queue; + + spin_lock_bh(&pstapriv->sta_hash_lock); + + if (_rtw_queue_empty23a(pfree_sta_queue)) { + spin_unlock_bh(&pstapriv->sta_hash_lock); + return NULL; + } + psta = container_of((&pfree_sta_queue->queue)->next, struct sta_info, list); + + list_del_init(&psta->list); + + tmp_aid = psta->aid; + + _rtw_init_stainfo(psta); + + psta->padapter = pstapriv->padapter; + + memcpy(psta->hwaddr, hwaddr, ETH_ALEN); + + index = wifi_mac_hash(hwaddr); + + RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_, + ("rtw_alloc_stainfo23a: index = %x", index)); + if (index >= NUM_STA) { + RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, + ("ERROR => rtw_alloc_stainfo23a: index >= NUM_STA")); + psta = NULL; + goto exit; + } + phash_list = &pstapriv->sta_hash[index]; + + list_add_tail(&psta->hash_list, phash_list); + + pstapriv->asoc_sta_count ++ ; + +/* 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_timer23a(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 = (NR_RECVBUFF-2); */ + preorder_ctrl->wsize_b = 64;/* 64; */ + + _rtw_init_queue23a(&preorder_ctrl->pending_recvframe_queue); + + rtw_init_recv_timer23a(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: + spin_unlock_bh(&pstapriv->sta_hash_lock); + return psta; +} + +/* using pstapriv->sta_hash_lock to protect */ +u32 rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta) +{ + struct rtw_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; + struct hw_xmit *phwxmit; + int i; + + if (psta == NULL) + goto exit; + + spin_lock_bh(&psta->lock); + psta->state &= ~_FW_LINKED; + spin_unlock_bh(&psta->lock); + + pfree_sta_queue = &pstapriv->free_sta_queue; + + pstaxmitpriv = &psta->sta_xmitpriv; + + spin_lock_bh(&pxmitpriv->lock); + + rtw_free_xmitframe_queue23a(pxmitpriv, &psta->sleep_q); + psta->sleepq_len = 0; + + /* vo */ + rtw_free_xmitframe_queue23a(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending); + list_del_init(&pstaxmitpriv->vo_q.tx_pending); + phwxmit = pxmitpriv->hwxmits; + phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt; + pstaxmitpriv->vo_q.qcnt = 0; + + /* vi */ + rtw_free_xmitframe_queue23a(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending); + list_del_init(&pstaxmitpriv->vi_q.tx_pending); + phwxmit = pxmitpriv->hwxmits+1; + phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt; + pstaxmitpriv->vi_q.qcnt = 0; + + /* be */ + rtw_free_xmitframe_queue23a(pxmitpriv, &pstaxmitpriv->be_q.sta_pending); + list_del_init(&pstaxmitpriv->be_q.tx_pending); + phwxmit = pxmitpriv->hwxmits+2; + phwxmit->accnt -= pstaxmitpriv->be_q.qcnt; + pstaxmitpriv->be_q.qcnt = 0; + + /* bk */ + rtw_free_xmitframe_queue23a(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending); + list_del_init(&pstaxmitpriv->bk_q.tx_pending); + phwxmit = pxmitpriv->hwxmits+3; + phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt; + pstaxmitpriv->bk_q.qcnt = 0; + + 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 will be init in alloc_stainfo */ + /* _rtw_init_sta_xmit_priv23a(&psta->sta_xmitpriv); */ + /* _rtw_init_sta_recv_priv23a(&psta->sta_recvpriv); */ + + del_timer_sync(&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 rtw_queue *ppending_recvframe_queue; + struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + + preorder_ctrl = &psta->recvreorder_ctrl[i]; + + del_timer_sync(&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_recvframe23a(prframe, pfree_recv_queue); + } + spin_unlock_bh(&ppending_recvframe_queue->lock); + } + if (!(psta->state & WIFI_AP_STATE)) + rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, false); +#ifdef CONFIG_8723AU_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 &= ~CHKBIT(psta->aid); + pstapriv->tim_bitmap &= ~CHKBIT(psta->aid); + + if ((psta->aid >0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) { + pstapriv->sta_aid[psta->aid - 1] = NULL; + psta->aid = 0; + } +#endif /* CONFIG_8723AU_AP_MODE */ + list_add_tail(&psta->list, get_list_head(pfree_sta_queue)); +exit: + return _SUCCESS; +} + +/* free all stainfo which in sta_hash[all] */ +void rtw_free_all_stainfo23a(struct rtw_adapter *padapter) +{ + struct list_head *plist, *phead, *ptmp; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info* pbcmc_stainfo = rtw_get_bcmc_stainfo23a(padapter); + s32 index; 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]; + + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, hash_list); + + if (pbcmc_stainfo!= psta) + rtw_free_stainfo23a(padapter, psta); + } + } + spin_unlock_bh(&pstapriv->sta_hash_lock); +} + +/* any station allocated can be searched by hash list */ +struct sta_info *rtw_get_stainfo23a(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_multicast_ether_addr(hwaddr)) + addr = bc_addr; + else + addr = hwaddr; + + index = wifi_mac_hash(addr); + + spin_lock_bh(&pstapriv->sta_hash_lock); + + phead = &pstapriv->sta_hash[index]; + + list_for_each(plist, phead) { + psta = container_of(plist, struct sta_info, hash_list); + + if (!memcmp(psta->hwaddr, addr, ETH_ALEN)) { + /* if found the matched address */ + break; + } + psta = NULL; + } + spin_unlock_bh(&pstapriv->sta_hash_lock); + return psta; +} + +u32 rtw_init_bcmc_stainfo23a(struct rtw_adapter* padapter) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + struct tx_servq *ptxservq; + u32 res = _SUCCESS; + unsigned char bcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + psta = rtw_alloc_stainfo23a(pstapriv, bcast_addr); + if (psta == NULL) { + res = _FAIL; + RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, + ("rtw_alloc_stainfo23a fail")); + return res; + } + /* default broadcast & multicast use macid 1 */ + psta->mac_id = 1; + + ptxservq = &psta->sta_xmitpriv.be_q; + return _SUCCESS; +} + +struct sta_info *rtw_get_bcmc_stainfo23a(struct rtw_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_stainfo23a(pstapriv, bc_addr); + return psta; +} + +u8 rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr) +{ + u8 res = true; +#ifdef CONFIG_8723AU_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 rtw_queue *pacl_node_q = &pacl_list->acl_node_q; + + spin_lock_bh(&pacl_node_q->lock); + phead = get_list_head(pacl_node_q); + + list_for_each(plist, phead) { + paclnode = container_of(plist, struct rtw_wlan_acl_node, list); + + 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/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c new file mode 100644 index 000000000000..76d2f240656e --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c @@ -0,0 +1,1760 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_WLAN_UTIL_C_ + +#include +#include +#include +#include + +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_IE23A[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20}; + +#define R2T_PHY_DELAY (0) + +/* define WAIT_FOR_BCN_TO_MIN (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_included23a(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_included23a(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_raid23a(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_type23a(struct rtw_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_included23a(rate, ratelen)) == true) + network_type |= WIRELESS_11B; + else if ((cckrates_included23a(rate, ratelen)) == true) + network_type |= WIRELESS_11BG; + else + network_type |= WIRELESS_11G; + } + return network_type; +} + +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; +} + +int is_basicrate(struct rtw_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; +} + +unsigned int ratetbl2rateset(struct rtw_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_set23a(struct rtw_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 UpdateBrateTbl23a(struct rtw_adapter *Adapter, u8 *mBratesOS) +{ + 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 = mBratesOS[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: + mBratesOS[i] |= IEEE80211_BASIC_RATE_MASK; + break; + default: + break; + } + } +} + +void Update23aTblForSoftAP(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_Flag23a(struct rtw_adapter *padapter) +{ + u8 bSaveFlag = true; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag)); +} + +void Restore_DM_Func_Flag23a(struct rtw_adapter *padapter) +{ + u8 bSaveFlag = false; + rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag)); +} + +void Switch_DM_Func23a(struct rtw_adapter *padapter, unsigned long mode, u8 enable) +{ + if (enable == true) + rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode)); + else + rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode)); +} + +static void Set_NETYPE0_MSR(struct rtw_adapter *padapter, u8 type) +{ + rtw_hal_set_hwreg23a(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type)); +} + +void Set_MSR23a(struct rtw_adapter *padapter, u8 type) +{ + Set_NETYPE0_MSR(padapter, type); +} + +inline u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_channel; +} + +inline void rtw_set_oper_ch23a(struct rtw_adapter *adapter, u8 ch) +{ + adapter_to_dvobj(adapter)->oper_channel = ch; +} + +inline u8 rtw_get_oper_bw23a(struct rtw_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_bwmode; +} + +inline void rtw_set_oper_bw23a(struct rtw_adapter *adapter, u8 bw) +{ + adapter_to_dvobj(adapter)->oper_bwmode = bw; +} + +inline u8 rtw_get_oper_ch23aoffset(struct rtw_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_ch_offset; +} + +inline void rtw_set_oper_ch23aoffset23a(struct rtw_adapter *adapter, u8 offset) +{ + adapter_to_dvobj(adapter)->oper_ch_offset = offset; +} + +void SelectChannel23a(struct rtw_adapter *padapter, unsigned char channel) +{ + mutex_lock(&adapter_to_dvobj(padapter)->setch_mutex); + + /* saved channel info */ + rtw_set_oper_ch23a(padapter, channel); + + rtw_hal_set_chan23a(padapter, channel); + + mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex); +} + +void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode, unsigned char channel_offset) +{ + mutex_lock(&adapter_to_dvobj(padapter)->setbw_mutex); + + /* saved bw info */ + rtw_set_oper_bw23a(padapter, bwmode); + rtw_set_oper_ch23aoffset23a(padapter, channel_offset); + + rtw_hal_set_bwmode23a(padapter, (enum ht_channel_width)bwmode, + channel_offset); + + mutex_unlock(&adapter_to_dvobj(padapter)->setbw_mutex); +} + +void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel, + unsigned char channel_offset, unsigned short bwmode) +{ + u8 center_ch; + + if (padapter->bNotifyChannelChange) + DBG_8723A("[%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)) { + /* SelectChannel23a(padapter, channel); */ + center_ch = channel; + } else { + /* switch to the proper channel */ + if (channel_offset == HAL_PRIME_CHNL_OFFSET_LOWER) { + /* SelectChannel23a(padapter, channel + 2); */ + center_ch = channel + 2; + } else { + /* SelectChannel23a(padapter, channel - 2); */ + center_ch = channel - 2; + } + } + + /* set Channel */ + mutex_lock(&adapter_to_dvobj(padapter)->setch_mutex); + + /* saved channel/bw info */ + rtw_set_oper_ch23a(padapter, channel); + rtw_set_oper_bw23a(padapter, bwmode); + rtw_set_oper_ch23aoffset23a(padapter, channel_offset); + + rtw_hal_set_chan23a(padapter, center_ch); /* set center channel */ + + mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex); + + SetBWMode23a(padapter, bwmode, channel_offset); +} + +int get_bsstype23a(unsigned short capability) +{ + if (capability & BIT(0)) + return WIFI_FW_AP_STATE; + else if (capability & BIT(1)) + return WIFI_FW_ADHOC_STATE; + return 0; +} + +inline u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork) +{ + return pnetwork->MacAddress; +} + +u16 get_beacon_interval23a(struct wlan_bssid_ex *bss) +{ + unsigned short val; + memcpy((unsigned char *)&val, rtw_get_beacon_interval23a_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); +} + +int is_client_associated_to_ap23a(struct rtw_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_ibss23a(struct rtw_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_empty23a(struct rtw_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_timeout23a(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_entry23a(struct rtw_adapter *Adapter, u8 ucIndex) +{ + rtw_hal_set_hwreg23a(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex)); +} + +void invalidate_cam_all23a(struct rtw_adapter *padapter) +{ + rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL); +} + +void write_cam23a(struct rtw_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_hwreg23a(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val); + + /* rtw_write32(padapter, WCAMI, val); */ + + /* cmd = CAM_POLLINIG | CAM_WRITE | (addr + j); */ + /* rtw_write32(padapter, RWCAM, cmd); */ + + /* DBG_8723A("%s => cam write: %x, %x\n", __func__, cmd, val); */ + + } +} + +void clear_cam_entry23a(struct rtw_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_cam23a(padapter, entry, 0, null_sta, null_key); +} + +int allocate_fw_sta_entry23a(struct rtw_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_entry23a(struct rtw_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL); + + memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info)); +} + +#if defined(CONFIG_8723AU_P2P) && defined(CONFIG_8723AU_P2P) +int WFD_info_handler(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE) +{ + struct wifidirect_info *pwdinfo; + u8 wfd_ie[128] = {0x00}; + u32 wfd_ielen = 0; + + pwdinfo = &padapter->wdinfo; + if (rtw_get_wfd_ie((u8 *) pIE, pIE->Length, wfd_ie, &wfd_ielen)) { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] Found WFD IE\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) { + pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + return true; + } + } else { + DBG_8723A("[%s] NO WFD IE\n", __func__); + } + return _FAIL; +} +#endif + +int WMM_param_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * 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 WMMOnAssocRsp23a(struct rtw_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_hwreg23a(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_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + /* acm_mask |= (ACM? BIT(0):0); */ + edca[XMIT_BK_QUEUE] = acParm; + break; + case 0x2: + rtw_hal_set_hwreg23a(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_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + acm_mask |= (ACM? BIT(3):0); + edca[XMIT_VO_QUEUE] = acParm; + break; + } + + DBG_8723A("WMM(%x): %x, %x\n", ACI, ACM, acParm); + } + + if (padapter->registrypriv.acm_method == 1) + rtw_hal_set_hwreg23a(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; + + /* 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_8723A("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]); + } + + return; +} + +static void bwmode_update_check(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE) +{ + 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; + unsigned char new_bwmode; + unsigned char new_ch_offset; + + if (!pIE) + return; + if (!phtpriv->ht_option) + 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_handler23a(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_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + + /* update ap's stainfo */ + psta = rtw_get_stainfo23a(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_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * 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 == false) return; + + pmlmeinfo->HT_caps_enable = 1; + + for (i = 0; i < (pIE->Length); i++) { + if (i != 2) { + /* Commented by Albert 2010/07/12 */ + /* 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; + } + } + + /* Commented by Albert 2010/07/12 */ + /* Have to handle the endian issue after copying. */ + /* HT_ext_caps didn't be used yet. */ + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info); + pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps); + + rtw23a_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_1R23A[i]; + else + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i]; + } + return; +} + +void HT_info_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * 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 == false) 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 HTOnAssocRsp23a(struct rtw_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_8723A("%s\n", __func__); + + if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) { + pmlmeinfo->HT_enable = 1; + } else { + pmlmeinfo->HT_enable = 0; + /* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + 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_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing)); + + rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); +} + +void ERP_IE_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * 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_update23a(struct rtw_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_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_len) +{ + unsigned int len; + unsigned char *p; + unsigned short val16; + struct wlan_network *cur_network = &Adapter->mlmepriv.cur_network; + 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; + 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; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe; + u8 *pbssid = hdr->addr3; + + if (is_client_associated_to_ap23a(Adapter) == false) + return true; + + len = packet_len - sizeof(struct ieee80211_hdr_3addr); + + if (len > MAX_IE_SZ) { + DBG_8723A("%s IE too long for survey event\n", __func__); + return _FAIL; + } + + if (memcmp(cur_network->network.MacAddress, pbssid, 6)) { + DBG_8723A("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT, + MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress)); + return true; + } + + bssid = (struct wlan_bssid_ex *)kzalloc(sizeof(struct wlan_bssid_ex), + GFP_ATOMIC); + + if (ieee80211_is_beacon(hdr->frame_control)) + bssid->reserved = 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 ieee80211_hdr_3addr)), bssid->IELength); + + /* check bw and channel offset */ + /* parsing HT_CAP_IE */ + p = rtw_get_ie23a(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 = pht_cap->cap_info; + } else { + ht_cap_info = 0; + } + /* parsing HT_INFO_IE */ + p = rtw_get_ie23a(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_8723A("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__, + ht_cap_info, ht_info_infos_0); + DBG_8723A("%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_8723A("%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 */ + } + + /* Checking for channel */ + p = rtw_get_ie23a(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_ie23a(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_8723A("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_8723A("%s beacon channel:%d cur channel:%d disconnect\n", __func__, + bcn_channel, Adapter->mlmeextpriv.cur_channel); + goto _mismatch; + } + + /* checking SSID */ + if ((p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_)) == NULL) { + DBG_8723A("%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.ssid_len = *(p + 1); + } else { + bssid->Ssid.ssid_len = 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.ssid_len, + cur_network->network.Ssid.ssid, + cur_network->network.Ssid.ssid_len)); + + if (memcmp(bssid->Ssid.ssid, cur_network->network.Ssid.ssid, 32) || + bssid->Ssid.ssid_len != cur_network->network.Ssid.ssid_len) { + if (bssid->Ssid.ssid[0] != '\0' && + bssid->Ssid.ssid_len != 0) { /* not hidden ssid */ + DBG_8723A("%s(), SSID is not match return FAIL\n", + __func__); + goto _mismatch; + } + } + + /* check encryption info */ + val16 = rtw_get_capability23a((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_8723A("%s(), privacy is not match return FAIL\n", __func__); + goto _mismatch; + } + + rtw_get_sec_ie23a(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_8723A("%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_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12); + if (pbuf && (wpa_ielen>0)) { + if (_SUCCESS == rtw_parse_wpa_ie23a(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_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12); + + if (pbuf && (wpa_ielen>0)) { + if (_SUCCESS == rtw_parse_wpa2_ie23a(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_8723A("%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_8723A("%s authentication is not match , return FAIL\n", __func__); + goto _mismatch; + } + } + + kfree(bssid); + return _SUCCESS; + +_mismatch: + kfree(bssid); + + return _FAIL; +} + +void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta) +{ + unsigned int i; + unsigned int len; + struct ndis_802_11_var_ies * pIE; + + len = pkt_len - + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)); + + for (i = 0; i < len;) { + pIE = (struct ndis_802_11_var_ies *)(pframe + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)) + i); + + switch (pIE->ElementID) { + case _HT_EXTRA_INFO_IE_: /* HT info */ + /* HT_info_handler23a(padapter, pIE); */ + bwmode_update_check(padapter, pIE); + break; + case _ERPINFO_IE_: + ERP_IE_handler23a(padapter, pIE); + VCS_update23a(padapter, psta); + break; + default: + break; + } + i += (pIE->Length + 2); + } +} + +unsigned int is_ap_in_tkip23a(struct rtw_adapter *padapter) +{ + u32 i; + struct ndis_802_11_var_ies * 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_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) { + pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((!memcmp(pIE->data, RTW_WPA_OUI23A, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER23A, 4))) + return true; + break; + case _RSN_IE_2_: + if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER23A, 4)) + return true; + break; + default: + break; + } + i += (pIE->Length + 2); + } + return false; + } else { + return false; + } +} + +unsigned int should_forbid_n_rate23a(struct rtw_adapter * padapter) +{ + u32 i; + struct ndis_802_11_var_ies * pIE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_bssid_ex *cur_network = &pmlmepriv->cur_network.network; + + if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(struct ndis_802_11_fixed_ies); i < cur_network->IELength;) { + pIE = (struct ndis_802_11_var_ies *)(cur_network->IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) && + ((!memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP23A, 4)) || + (!memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP23A, 4)))) + return false; + break; + case _RSN_IE_2_: + if ((!memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP23A, 4)) || + (!memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP23A, 4))) + return false; + default: + break; + } + + i += (pIE->Length + 2); + } + return true; + } else { + return false; + } +} + +unsigned int is_ap_in_wep23a(struct rtw_adapter *padapter) +{ + u32 i; + struct ndis_802_11_var_ies * 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_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) { + pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4)) + return false; + break; + case _RSN_IE_2_: + return false; + + default: + break; + } + + i += (pIE->Length + 2); + } + + return true; + } else { + return false; + } +} + +int wifirate2_ratetbl_inx23a(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_rate23a(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_inx23a(*(ptn + i)); + } + return mask; +} + +unsigned int update_supported_rate23a(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_inx23a(*(ptn + i)); + return mask; +} + +unsigned int update_MSC_rate23a(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_GI23a(struct rtw_adapter *padapter, + struct HT_caps_element *pHT_caps) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + unsigned char bit_offset; + + 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 (pHT_caps->u.HT_cap_element.HT_caps_info & (0x1 << bit_offset)) + return _SUCCESS; + else + return _FAIL; +} + +unsigned char get_highest_rate_idx23a(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; +} + +unsigned char get_highest_mcs_rate(struct HT_caps_element *pHT_caps) +{ + int i, mcs_rate; + + mcs_rate = (pHT_caps->u.HT_cap_element.MCS_rate[0] | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 8)); + + for (i = 15; i >= 0; i--) { + if (mcs_rate & (0x1 << i)) + break; + } + return i; +} + +void Update_RA_Entry23a(struct rtw_adapter *padapter, struct sta_info *psta) +{ + rtw_hal_update_ra_mask23a(psta, 0); +} + +void enable_rate_adaptive(struct rtw_adapter *padapter, struct sta_info *psta) +{ + Update_RA_Entry23a(padapter, psta); +} + +void set_sta_rate23a(struct rtw_adapter *padapter, struct sta_info *psta) +{ + /* rate adaptive */ + enable_rate_adaptive(padapter, psta); +} + +/* Update RRSR and Rate for USERATE */ +void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 wirelessmode) +{ + unsigned char supported_rates[NDIS_802_11_LENGTH_RATES_EX]; +#ifdef CONFIG_8723AU_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_8723AU_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_rate23a(padapter, IEEE80211_CCK_RATE_1MB); + else + update_mgnt_tx_rate23a(padapter, IEEE80211_OFDM_RATE_6MB); + + rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, supported_rates); +} + +unsigned char check_assoc_AP23a(u8 *pframe, uint len) +{ + unsigned int i; + struct ndis_802_11_var_ies * 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_ies); i < len;) { + pIE = (struct ndis_802_11_var_ies *)(pframe + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) || + (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) { + DBG_8723A("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_8723A("link to Broadcom AP\n"); + return HT_IOT_PEER_BROADCOM; + } else if (!memcmp(pIE->data, MARVELL_OUI, 3)) { + DBG_8723A("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_8723A("link to Ralink AP\n"); + return HT_IOT_PEER_RALINK; + } + } else if (!memcmp(pIE->data, CISCO_OUI, 3)) { + DBG_8723A("link to Cisco AP\n"); + return HT_IOT_PEER_CISCO; + } else if (!memcmp(pIE->data, REALTEK_OUI, 3)) { + DBG_8723A("link to Realtek 96B\n"); + return HT_IOT_PEER_REALTEK; + } else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) { + DBG_8723A("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_8723A("link to Tenda W311R AP\n"); + return HT_IOT_PEER_TENDA; + } else { + DBG_8723A("Capture EPIGRAM_OUI\n"); + } + } else { + break; + } + default: + break; + } + + i += (pIE->Length + 2); + } + + if (ralink_vendor_flag && !epigram_vendor_flag) { + DBG_8723A("link to Ralink AP\n"); + return HT_IOT_PEER_RALINK; + } else if (ralink_vendor_flag && epigram_vendor_flag) { + DBG_8723A("link to Tenda W311R AP\n"); + return HT_IOT_PEER_TENDA; + } else { + DBG_8723A("link to new AP\n"); + return HT_IOT_PEER_UNKNOWN; + } +} + +void update_IOT_info23a(struct rtw_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_Func23a(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_Func23a(padapter, ~DYNAMIC_BB_DYNAMIC_TXPWR, + false); + break; + default: + pmlmeinfo->turboMode_cts2self = 0; + pmlmeinfo->turboMode_rtsen = 1; + break; + } +} + +void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap) +{ + struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + bool ShortPreamble; + + 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_hwreg23a(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_hwreg23a(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_hwreg23a(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime); +} + +void update_wireless_mode23a(struct rtw_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_len23a(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_included23a(rate, ratelen)) == true) + network_type |= WIRELESS_11B; + else if ((cckrates_included23a(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_rate23a(padapter, IEEE80211_CCK_RATE_1MB); + else + update_mgnt_tx_rate23a(padapter, IEEE80211_OFDM_RATE_6MB); +} + +void update_bmc_sta_support_rate23a(struct rtw_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, 4); + } +} + +int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx) +{ + unsigned int ie_len; + struct ndis_802_11_var_ies *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_ies *)rtw_get_ie23a(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_ies *)rtw_get_ie23a(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_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr) +{ + struct sta_info *psta; + u16 tid, start_seq, 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_stainfo23a(pstapriv, addr); + + if (psta) { + start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4; + + 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)? true :false; + } +} + +void update_TSF23a(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len) +{ + u8 *pIE; + u32 *pbuf; + + pIE = pframe + sizeof(struct ieee80211_hdr_3addr); + pbuf = (u32 *)pIE; + + pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1)); + + pmlmeext->TSFValue = pmlmeext->TSFValue << 32; + + pmlmeext->TSFValue |= le32_to_cpu(*pbuf); +} + +void correct_TSF23a(struct rtw_adapter *padapter, struct mlme_ext_priv *pmlmeext) +{ + rtw_hal_set_hwreg23a(padapter, HW_VAR_CORRECT_TSF, NULL); +} + +void beacon_timing_control23a(struct rtw_adapter *padapter) +{ + rtw_hal_bcn_related_reg_setting23a(padapter); +} + +static struct rtw_adapter *pbuddy_padapter; + +int rtw_handle_dualmac23a(struct rtw_adapter *adapter, bool init) +{ + int status = _SUCCESS; + + if (init) { + if (pbuddy_padapter == NULL) { + pbuddy_padapter = adapter; + DBG_8723A("%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_8723A("%s(): pbuddy_padapter exist, Exchange Information\n", __func__); + } + } else { + pbuddy_padapter = NULL; + } + return status; +} diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c new file mode 100644 index 000000000000..a1abba053944 --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_xmit.c @@ -0,0 +1,2463 @@ +/****************************************************************************** + * + * 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. + * + ******************************************************************************/ +#define _RTW_XMIT_C_ + +#include +#include +#include +#include +#include +#include + +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_queue23a(&ptxservq->sta_pending); + ptxservq->qcnt = 0; + +} + +void _rtw_init_sta_xmit_priv23a(struct sta_xmit_priv *psta_xmitpriv) +{ + + spin_lock_init(&psta_xmitpriv->lock); + + /* for (i = 0 ; i < MAX_NUMBLKS; i++) */ + /* _init_txservq(&psta_xmitpriv->blk_q[i]); */ + + _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_priv23a(struct xmit_priv *pxmitpriv, struct rtw_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(). */ + /* memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */ + + spin_lock_init(&pxmitpriv->lock); + spin_lock_init(&pxmitpriv->lock_sctx); + sema_init(&pxmitpriv->xmit_sema, 0); + sema_init(&pxmitpriv->terminate_xmitthread_sema, 0); + + /* + Please insert all the queue initializaiton using _rtw_init_queue23a below + */ + + pxmitpriv->adapter = padapter; + + _rtw_init_queue23a(&pxmitpriv->be_pending); + _rtw_init_queue23a(&pxmitpriv->bk_pending); + _rtw_init_queue23a(&pxmitpriv->vi_pending); + _rtw_init_queue23a(&pxmitpriv->vo_pending); + _rtw_init_queue23a(&pxmitpriv->bm_pending); + + _rtw_init_queue23a(&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 = PTR_ALIGN(pxmitpriv->pallocated_frame_buf, 4); + + 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_queue23a(&pxmitpriv->free_xmitbuf_queue); + INIT_LIST_HEAD(&pxmitpriv->xmitbuf_list); + _rtw_init_queue23a(&pxmitpriv->pending_xmitbuf_queue); + + for (i = 0; i < NR_XMITBUFF; i++) { + pxmitbuf = kzalloc(sizeof(struct xmit_buf), GFP_KERNEL); + if (!pxmitbuf) + goto fail; + INIT_LIST_HEAD(&pxmitbuf->list); + INIT_LIST_HEAD(&pxmitbuf->list2); + + pxmitbuf->padapter = padapter; + + /* Tx buf allocation may fail sometimes, so sleep and retry. */ + res = rtw_os_xmit_resource_alloc23a(padapter, pxmitbuf, + (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ)); + if (res == _FAIL) { + goto fail; + } + + list_add_tail(&pxmitbuf->list, + &pxmitpriv->free_xmitbuf_queue.queue); + list_add_tail(&pxmitbuf->list2, + &pxmitpriv->xmitbuf_list); + } + + pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF; + + /* init xframe_ext queue, the same count as extbuf */ + _rtw_init_queue23a(&pxmitpriv->free_xframe_ext_queue); + + pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_frame) + 4); + + if (pxmitpriv->xframe_ext_alloc_addr == NULL) { + pxmitpriv->xframe_ext = NULL; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n")); + res = _FAIL; + goto exit; + } + pxmitpriv->xframe_ext = PTR_ALIGN(pxmitpriv->xframe_ext_alloc_addr, 4); + pxframe = (struct xmit_frame*)pxmitpriv->xframe_ext; + + for (i = 0; i < num_xmit_extbuf; i++) { + INIT_LIST_HEAD(&pxframe->list); + + pxframe->padapter = padapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + pxframe->ext_tag = 1; + + list_add_tail(&pxframe->list, + &pxmitpriv->free_xframe_ext_queue.queue); + + pxframe++; + } + pxmitpriv->free_xframe_ext_cnt = num_xmit_extbuf; + + /* Init xmit extension buff */ + _rtw_init_queue23a(&pxmitpriv->free_xmit_extbuf_queue); + INIT_LIST_HEAD(&pxmitpriv->xmitextbuf_list); + + for (i = 0; i < num_xmit_extbuf; i++) { + pxmitbuf = kzalloc(sizeof(struct xmit_buf), GFP_KERNEL); + if (!pxmitbuf) + goto fail; + INIT_LIST_HEAD(&pxmitbuf->list); + INIT_LIST_HEAD(&pxmitbuf->list2); + + pxmitbuf->padapter = padapter; + + /* Tx buf allocation may fail sometimes, so sleep and retry. */ + res = rtw_os_xmit_resource_alloc23a(padapter, pxmitbuf, + max_xmit_extbuf_size + XMITBUF_ALIGN_SZ); + if (res == _FAIL) { + goto exit; + } + + list_add_tail(&pxmitbuf->list, + &pxmitpriv->free_xmit_extbuf_queue.queue); + list_add_tail(&pxmitbuf->list2, + &pxmitpriv->xmitextbuf_list); + } + + pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf; + + rtw_alloc_hwxmits23a(padapter); + rtw_init_hwxmits23a(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; + mutex_init(&pxmitpriv->ack_tx_mutex); + rtw_sctx_init23a(&pxmitpriv->ack_tx_ops, 0); + rtw_hal_init23a_xmit_priv(padapter); + +exit: + + return res; +fail: + goto exit; +} + +void _rtw_free_xmit_priv23a (struct xmit_priv *pxmitpriv) +{ + struct rtw_adapter *padapter = pxmitpriv->adapter; + struct xmit_frame *pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf; + struct xmit_buf *pxmitbuf; + struct list_head *plist, *ptmp; + u32 num_xmit_extbuf = NR_XMIT_EXTBUFF; + int i; + + rtw_hal_free_xmit_priv23a(padapter); + + if (pxmitpriv->pxmit_frame_buf == NULL) + return; + for (i = 0; i < NR_XMITFRAME; i++) { + rtw_os_xmit_complete23a(padapter, pxmitframe); + pxmitframe++; + } + + list_for_each_safe(plist, ptmp, &pxmitpriv->xmitbuf_list) { + pxmitbuf = container_of(plist, struct xmit_buf, list2); + list_del_init(&pxmitbuf->list2); + rtw_os_xmit_resource_free23a(padapter, pxmitbuf); + kfree(pxmitbuf); + } + + if (pxmitpriv->pallocated_frame_buf) { + rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4); + } + + /* free xframe_ext queue, the same count as extbuf */ + if ((pxmitframe = (struct xmit_frame*)pxmitpriv->xframe_ext)) { + for (i = 0; ixframe_ext_alloc_addr) + rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, num_xmit_extbuf * sizeof(struct xmit_frame) + 4); + + /* free xmit extension buff */ + list_for_each_safe(plist, ptmp, &pxmitpriv->xmitextbuf_list) { + pxmitbuf = container_of(plist, struct xmit_buf, list2); + list_del_init(&pxmitbuf->list2); + rtw_os_xmit_resource_free23a(padapter, pxmitbuf); + kfree(pxmitbuf); + } + + rtw_free_hwxmits23a(padapter); + mutex_destroy(&pxmitpriv->ack_tx_mutex); +} + +static void update_attrib_vcs_info(struct rtw_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->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]); + } + + if (psta == NULL) { + DBG_8723A("%s, psta == NUL\n", __func__); + return; + } + + if (!(psta->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return; + } + + 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_acm23a(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_8723A("qos_acm23a(): 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 UserPriority = 0; + + _rtw_open_pktfile23a(ppktfile->pkt, ppktfile); + _rtw_pktfile_read23a(ppktfile, (unsigned char*)ðerhdr, ETH_HLEN); + + /* get UserPriority from IP hdr */ + if (pattrib->ether_type == 0x0800) { + _rtw_pktfile_read23a(ppktfile, (u8*)&ip_hdr, sizeof(ip_hdr)); +/* UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */ + UserPriority = 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." */ + UserPriority = 7; + } + + pattrib->priority = UserPriority; + pattrib->hdrlen = sizeof(struct ieee80211_qos_hdr); + pattrib->subtype = WIFI_QOS_DATA_TYPE; +} + +static s32 update_attrib(struct rtw_adapter *padapter, + struct sk_buff *pkt, struct pkt_attrib *pattrib) +{ + uint i; + 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_pktfile23a(pkt, &pktfile); + i = _rtw_pktfile_read23a(&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) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) { + 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 (pattrib->ether_type == ETH_P_IP) { + /* 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_read23a(&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")); + pattrib->dhcp_pkt = 1; + } + } + } + } else if (0x888e == pattrib->ether_type) { + DBG_8723A_LEVEL(_drv_always_, "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_cmd23a(padapter, LPS_CTRL_SPECIAL_PACKET, 1); + } + + bmcast = is_multicast_ether_addr(pattrib->ra); + + /* get sta_info */ + if (bmcast) { + psta = rtw_get_bcmc_stainfo23a(padapter); + } else { + psta = rtw_get_stainfo23a(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:" + MAC_FMT"\n", MAC_ARG(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_8723A("%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:" MAC_FMT + "\n", MAC_ARG(pattrib->ra))); + res = _FAIL; + goto exit; + } + + pattrib->ack_policy = 0; + /* get ether_hdr_len */ + + /* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */ + pattrib->pkt_hdrlen = ETH_HLEN; + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + 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_acm23a(pmlmepriv->acm_mask, + pattrib->priority); + } + } + } + + if (psta->ieee8021x_blocked == true) { + 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) == false)) { + 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.busetkip" + "key(%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\n", pattrib->encrypt)); + + if (pattrib->encrypt && psecuritypriv->hw_decrypted == false) { + pattrib->bswenc = true; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("update_attrib: encrypt =%d bswenc = true\n", + pattrib->encrypt)); + } else { + pattrib->bswenc = false; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, + ("update_attrib: bswenc = false\n")); + } + update_attrib_phy_info(pattrib, psta); + +exit: + + return res; +} + +static s32 xmitframe_addmic(struct rtw_adapter *padapter, + struct xmit_frame *pxmitframe) { + 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; + int curfragnum, length; + u8 *pframe, *payload, mic[8]; + u8 priority[4]= {0x0, 0x0, 0x0, 0x0}; + u8 hw_hdr_offset = 0; + int bmcst = is_multicast_ether_addr(pattrib->ra); + + if (pattrib->psta) { + stainfo = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + stainfo = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]); + } + + if (!stainfo) { + DBG_8723A("%s, psta == NUL\n", __func__); + return _FAIL; + } + + if (!(stainfo->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", + __func__, stainfo->state); + return _FAIL; + } + + hw_hdr_offset = TXDESC_OFFSET; + + if (pattrib->encrypt == _TKIP_) { + /* encode mic code */ + if (stainfo) { + 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_secmicsetkey23a(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey); + } else { + if (!memcmp(&stainfo->dot11tkiptxmickey.skey[0], + null_key, 16)) { + return _FAIL; + } + /* start to calculate the mic code */ + rtw_secmicsetkey23a(&micdata, &stainfo->dot11tkiptxmickey.skey[0]); + } + + if (pframe[1] & 1) { /* ToDS == 1 */ + /* DA */ + rtw_secmicappend23a(&micdata, &pframe[16], 6); + if (pframe[1] & 2) /* From Ds == 1 */ + rtw_secmicappend23a(&micdata, + &pframe[24], 6); + else + rtw_secmicappend23a(&micdata, + &pframe[10], 6); + } else { /* ToDS == 0 */ + /* DA */ + rtw_secmicappend23a(&micdata, &pframe[4], 6); + if (pframe[1] & 2) /* From Ds == 1 */ + rtw_secmicappend23a(&micdata, + &pframe[16], 6); + else + rtw_secmicappend23a(&micdata, + &pframe[10], 6); + } + + /* if (pqospriv->qos_option == 1) */ + if (pattrib->qos_en) + priority[0] = (u8)pxmitframe->attrib.priority; + + rtw_secmicappend23a(&micdata, &priority[0], 4); + + payload = pframe; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; + curfragnum++) { + payload = (u8 *)RND4((unsigned long)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_secmicappend23a(&micdata, payload, + length); + payload = payload + length; + } else { + length = pxmitpriv->frag_len - + pattrib->hdrlen - + pattrib->iv_len - + ((pattrib->bswenc) ? + pattrib->icv_len : 0); + rtw_secmicappend23a(&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_secgetmic23a(&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_stainfo23a ==" + "NULL!!!\n")); + } + } + + return _SUCCESS; +} + +static s32 xmitframe_swencrypt(struct rtw_adapter *padapter, + struct xmit_frame *pxmitframe) +{ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + /* if ((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */ + if (pattrib->bswenc) { + /* DBG_8723A("start xmitframe_swencrypt\n"); */ + RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, + ("### xmitframe_swencrypt\n")); + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + rtw_wep_encrypt23a(padapter, pxmitframe); + break; + case _TKIP_: + rtw_tkip_encrypt23a(padapter, pxmitframe); + break; + case _AES_: + rtw_aes_encrypt23a(padapter, pxmitframe); + break; + default: + break; + } + + } else { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, + ("### xmitframe_hwencrypt\n")); + } + + return _SUCCESS; +} + +s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr, + struct pkt_attrib *pattrib) +{ + u16 *qc; + + struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + u8 qos_option = false; + int res = _SUCCESS; + u16 *fctrl = &pwlanhdr->frame_control; + + struct sta_info *psta; + + int bmcst = is_multicast_ether_addr(pattrib->ra); + + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + if (bmcst) { + psta = rtw_get_bcmc_stainfo23a(padapter); + } else { + psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra); + } + } + + if (psta == NULL) { + DBG_8723A("%s, psta == NUL\n", __func__); + return _FAIL; + } + + if (!(psta->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return _FAIL; + } + + 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) == true)) { + /* 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) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) { + 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 & CHKBIT(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)) { + /* DBG_8723A("tx ampdu seqnum(%d) < tx_seq(%d)\n", 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 { + /* DBG_8723A("tx ampdu over run\n"); */ + psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff; + pattrib->ampdu_en = true;/* AGG EN */ + } + } + } + } +exit: + return res; +} + +s32 rtw_txframes_pending23a(struct rtw_adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + return (!_rtw_queue_empty23a(&pxmitpriv->be_pending)) || + (!_rtw_queue_empty23a(&pxmitpriv->bk_pending)) || + (!_rtw_queue_empty23a(&pxmitpriv->vi_pending)) || + (!_rtw_queue_empty23a(&pxmitpriv->vo_pending)); +} + +s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter, + struct pkt_attrib *pattrib) +{ + struct sta_info *psta; + struct tx_servq *ptxservq; + int priority = pattrib->priority; + + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]); + } + if (psta == NULL) { + DBG_8723A("%s, psta == NUL\n", __func__); + return 0; + } + if (!(psta->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, + psta->state); + return 0; + } + 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; + } + return ptxservq->qcnt; +} + +/* + * Calculate wlan 802.11 packet MAX size from pkt_attrib + * This function doesn't consider fragment case + */ +u32 rtw_calculate_wlan_pkt_size_by_attribue23a(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_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt, + struct xmit_frame *pxmitframe) +{ + struct pkt_file pktfile; + struct sta_info *psta; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz; + unsigned long addr; + u8 *pframe, *mem_start; + u8 hw_hdr_offset; + u8 *pbuf_start; + + s32 bmcst = is_multicast_ether_addr(pattrib->ra); + s32 res = _SUCCESS; + + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra); + } + + if (psta == NULL) { + DBG_8723A("%s, psta == NUL\n", __func__); + return _FAIL; + } + + if (!(psta->state &_FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return _FAIL; + } + + if (pxmitframe->buf_addr == NULL) { + DBG_8723A("==> %s buf_addr == NULL\n", __func__); + return _FAIL; + } + + pbuf_start = pxmitframe->buf_addr; + + hw_hdr_offset = TXDESC_OFFSET; + + mem_start = pbuf_start + hw_hdr_offset; + + if (rtw_make_wlanhdr23a(padapter, mem_start, pattrib) == _FAIL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("rtw_xmitframe_coalesce23a: rtw_make_wlanhdr23a " + "fail; drop pkt\n")); + res = _FAIL; + goto exit; + } + + _rtw_open_pktfile23a(pkt, &pktfile); + _rtw_pktfile_read23a(&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_xmiaframe_coalesce23a: 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_snap23a(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_read23a(&pktfile, pframe, pattrib->pktlen); + } else { + mem_sz = _rtw_pktfile_read23a(&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_endofpktfile23a(&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 = (unsigned long)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_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"); + res = _FAIL; + goto exit; + } + + xmitframe_swencrypt(padapter, pxmitframe); + + if (bmcst == false) + 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_snap23a(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]; + *(u16 *)(data + SNAP_SIZE) = htons(h_proto); + return SNAP_SIZE + sizeof(u16); +} + +void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + uint protection; + u8 *perp; + int erp_len; + + switch (pxmitpriv->vcs_setting) { + case DISABLE_VCS: + pxmitpriv->vcs = NONE_VCS; + break; + case ENABLE_VCS: + break; + case AUTO_VCS: + default: + perp = rtw_get_ie23a(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_stats23a(struct rtw_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++; + + psta = pxmitframe->attrib.psta; + if (psta) { + pstats = &psta->sta_stats; + pstats->tx_pkts++; + pstats->tx_bytes += sz; + } + } +} + +struct xmit_buf *rtw_alloc_xmitbuf23a_ext(struct xmit_priv *pxmitpriv) +{ + unsigned long irqL; + struct xmit_buf *pxmitbuf = NULL; + struct list_head *phead; + struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue; + + spin_lock_irqsave(&pfree_queue->lock, irqL); + + phead = get_list_head(pfree_queue); + + if (!list_empty(phead)) { + pxmitbuf = list_first_entry(phead, struct xmit_buf, list); + + list_del_init(&pxmitbuf->list); + + pxmitpriv->free_xmit_extbuf_cnt--; + pxmitbuf->priv_data = NULL; + pxmitbuf->ext_tag = true; + + if (pxmitbuf->sctx) { + DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } + + spin_unlock_irqrestore(&pfree_queue->lock, irqL); + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + unsigned long irqL; + struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue; + + if (pxmitbuf == NULL) + return _FAIL; + + spin_lock_irqsave(&pfree_queue->lock, irqL); + + 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, irqL); + + return _SUCCESS; +} + +struct xmit_buf *rtw_alloc_xmitbuf23a(struct xmit_priv *pxmitpriv) +{ + unsigned long irqL; + struct xmit_buf *pxmitbuf = NULL; + struct list_head *phead; + struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + /* DBG_8723A("+rtw_alloc_xmitbuf23a\n"); */ + + spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL); + + phead = get_list_head(pfree_xmitbuf_queue); + + if (!list_empty(phead)) { + pxmitbuf = list_first_entry(phead, struct xmit_buf, list); + + list_del_init(&pxmitbuf->list); + + pxmitpriv->free_xmitbuf_cnt--; + pxmitbuf->priv_data = NULL; + pxmitbuf->ext_tag = false; + pxmitbuf->flags = XMIT_VO_QUEUE; + + if (pxmitbuf->sctx) { + DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } + + spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL); + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + unsigned long irqL; + struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + /* DBG_8723A("+rtw_free_xmitbuf23a\n"); */ + + if (pxmitbuf == NULL) + return _FAIL; + + if (pxmitbuf->sctx) { + DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE); + } + + if (pxmitbuf->ext_tag) { + rtw_free_xmitbuf_ext23a(pxmitpriv, pxmitbuf); + } else { + spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL); + + 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, irqL); + } + + return _SUCCESS; +} + +static void rtw_init_xmitframe(struct xmit_frame *pxframe) +{ + if (pxframe != NULL) { + /* default value setting */ + 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->ack_report = 0; + } +} + +/* +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_xmitframe23a(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 rtw_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue; + + spin_lock_bh(&pfree_xmit_queue->lock); + + if (_rtw_queue_empty23a(pfree_xmit_queue) == true) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a:%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); + pxmitpriv->free_xmitframe_cnt--; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt)); + } + + spin_unlock_bh(&pfree_xmit_queue->lock); + + rtw_init_xmitframe(pxframe); + + return pxframe; +} + +struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pxframe = NULL; + struct list_head *plist, *phead; + struct rtw_queue *queue = &pxmitpriv->free_xframe_ext_queue; + + spin_lock_bh(&queue->lock); + + if (_rtw_queue_empty23a(queue) == true) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext:%d\n", pxmitpriv->free_xframe_ext_cnt)); + pxframe = NULL; + } else { + phead = get_list_head(queue); + plist = phead->next; + pxframe = container_of(plist, struct xmit_frame, list); + + list_del_init(&pxframe->list); + pxmitpriv->free_xframe_ext_cnt--; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt)); + } + + spin_unlock_bh(&queue->lock); + + rtw_init_xmitframe(pxframe); + + return pxframe; +} + +s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe) +{ + struct rtw_queue *queue = NULL; + struct rtw_adapter *padapter = pxmitpriv->adapter; + struct sk_buff *pndis_pkt = NULL; + + if (pxmitframe == NULL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("====== rtw_free_xmitframe23a():pxmitframe == NULL!!!!!!!!!!\n")); + goto exit; + } + + if (pxmitframe->pkt) { + pndis_pkt = pxmitframe->pkt; + pxmitframe->pkt = NULL; + } + + if (pxmitframe->ext_tag == 0) + queue = &pxmitpriv->free_xmit_queue; + else if (pxmitframe->ext_tag == 1) + queue = &pxmitpriv->free_xframe_ext_queue; + + if (!queue) + goto check_pkt_complete; + spin_lock_bh(&queue->lock); + + list_del_init(&pxmitframe->list); + list_add_tail(&pxmitframe->list, get_list_head(queue)); + if (pxmitframe->ext_tag == 0) { + pxmitpriv->free_xmitframe_cnt++; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt)); + } else if (pxmitframe->ext_tag == 1) { + pxmitpriv->free_xframe_ext_cnt++; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt)); + } + + spin_unlock_bh(&queue->lock); + +check_pkt_complete: + + if (pndis_pkt) + rtw_os_pkt_complete23a(padapter, pndis_pkt); + +exit: + + return _SUCCESS; +} + +void rtw_free_xmitframe_queue23a(struct xmit_priv *pxmitpriv, + struct rtw_queue *pframequeue) +{ + struct list_head *plist, *phead, *ptmp; + struct xmit_frame *pxmitframe; + + spin_lock_bh(&pframequeue->lock); + + phead = get_list_head(pframequeue); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, struct xmit_frame, list); + + rtw_free_xmitframe23a(pxmitpriv, pxmitframe); + } + spin_unlock_bh(&pframequeue->lock); + +} + +s32 rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter, + struct xmit_frame *pxmitframe) +{ + if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("rtw_xmitframe_enqueue23a: drop xmit pkt for " + "classifier fail\n")); + return _FAIL; + } + + return _SUCCESS; +} + +static struct xmit_frame * +dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, + struct tx_servq *ptxservq, struct rtw_queue *pframe_queue) +{ + struct list_head *phead; + struct xmit_frame *pxmitframe = NULL; + + phead = get_list_head(pframe_queue); + + if (!list_empty(phead)) { + pxmitframe = list_first_entry(phead, struct xmit_frame, list); + list_del_init(&pxmitframe->list); + ptxservq->qcnt--; + } + return pxmitframe; +} + +struct xmit_frame * +rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, + int entry) +{ + struct list_head *sta_plist, *sta_phead, *ptmp; + struct hw_xmit *phwxmit; + struct tx_servq *ptxservq = NULL; + struct rtw_queue *pframe_queue = NULL; + struct xmit_frame *pxmitframe = NULL; + struct rtw_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); + + list_for_each_safe(sta_plist, ptmp, sta_phead) { + 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 is no pending packets. */ + if (_rtw_queue_empty23a(pframe_queue)) /* must be done after get_next and before break */ + list_del_init(&ptxservq->tx_pending); + goto exit; + } + } + } +exit: + spin_unlock_bh(&pxmitpriv->lock); + return pxmitframe; +} + +struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta_info *psta, int up, u8 *ac) +{ + struct tx_servq *ptxservq = NULL; + + 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_pending23a : 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_pending23a : 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_pending23a : 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_pending23a : BE\n")); + break; + } + return ptxservq; +} + +/* + * Will enqueue pxmitframe to the proper queue, + * and indicate it to xx_pending list..... + */ +s32 rtw_xmit23a_classifier(struct rtw_adapter *padapter, + struct xmit_frame *pxmitframe) +{ + 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; + u8 ac_index; + int res = _SUCCESS; + + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(pstapriv, pattrib->ra); + } + if (psta == NULL) { + res = _FAIL; + DBG_8723A("rtw_xmit23a_classifier: psta == NULL\n"); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("rtw_xmit23a_classifier: psta == NULL\n")); + goto exit; + } + if (!(psta->state & _FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, + psta->state); + return _FAIL; + } + ptxservq = rtw_get_sta_pending23a(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_hwxmits23a(struct rtw_adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + int size; + + pxmitpriv->hwxmit_entry = HWXMIT_ENTRY; + + size = sizeof(struct hw_xmit) * (pxmitpriv->hwxmit_entry + 1); + pxmitpriv->hwxmits = kzalloc(size, GFP_KERNEL); + + hwxmits = pxmitpriv->hwxmits; + + if (pxmitpriv->hwxmit_entry == 5) { + /* pxmitpriv->bmc_txqueue.head = 0; */ + /* hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue; */ + hwxmits[0] .sta_queue = &pxmitpriv->bm_pending; + + /* pxmitpriv->vo_txqueue.head = 0; */ + /* hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue; */ + hwxmits[1] .sta_queue = &pxmitpriv->vo_pending; + + /* pxmitpriv->vi_txqueue.head = 0; */ + /* hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue; */ + hwxmits[2] .sta_queue = &pxmitpriv->vi_pending; + + /* pxmitpriv->bk_txqueue.head = 0; */ + /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */ + hwxmits[3] .sta_queue = &pxmitpriv->bk_pending; + + /* pxmitpriv->be_txqueue.head = 0; */ + /* hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue; */ + hwxmits[4] .sta_queue = &pxmitpriv->be_pending; + + } else if (pxmitpriv->hwxmit_entry == 4) { + + /* pxmitpriv->vo_txqueue.head = 0; */ + /* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */ + hwxmits[0] .sta_queue = &pxmitpriv->vo_pending; + + /* pxmitpriv->vi_txqueue.head = 0; */ + /* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */ + hwxmits[1] .sta_queue = &pxmitpriv->vi_pending; + + /* pxmitpriv->be_txqueue.head = 0; */ + /* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */ + hwxmits[2] .sta_queue = &pxmitpriv->be_pending; + + /* pxmitpriv->bk_txqueue.head = 0; */ + /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */ + hwxmits[3] .sta_queue = &pxmitpriv->bk_pending; + } else { + + } +} + +void rtw_free_hwxmits23a(struct rtw_adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + hwxmits = pxmitpriv->hwxmits; + kfree(hwxmits); +} + +void rtw_init_hwxmits23a(struct hw_xmit *phwxmit, int entry) +{ + int i; + + for (i = 0; i < entry; i++, phwxmit++) + phwxmit->accnt = 0; +} + +u32 rtw_get_ff_hwaddr23a(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 rtw_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 + */ +int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_frame *pxmitframe = NULL; + s32 res; + + pxmitframe = rtw_alloc_xmitframe23a(pxmitpriv); + + if (pxmitframe == NULL) { + RT_TRACE(_module_xmit_osdep_c_, _drv_err_, + ("rtw_xmit23a: no more pxmitframe\n")); + return -1; + } + + res = update_attrib(padapter, skb, &pxmitframe->attrib); + + if (res == _FAIL) { + RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit23a: update attrib fail\n")); + rtw_free_xmitframe23a(pxmitpriv, pxmitframe); + return -1; + } + pxmitframe->pkt = skb; + + rtw_led_control(padapter, LED_CTL_TX); + + do_queue_select(padapter, &pxmitframe->attrib); + +#ifdef CONFIG_8723AU_AP_MODE + spin_lock_bh(&pxmitpriv->lock); + if (xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe)) { + spin_unlock_bh(&pxmitpriv->lock); + return 1; + } + spin_unlock_bh(&pxmitpriv->lock); +#endif + + if (rtw_hal_xmit23a(padapter, pxmitframe) == false) + return 1; + + return 0; +} + +#if defined(CONFIG_8723AU_AP_MODE) + +int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_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_multicast_ether_addr(pattrib->ra); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false) + return ret; + + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__); + psta = rtw_get_stainfo23a(pstapriv, pattrib->ra); + } + + if (psta == NULL) { + DBG_8723A("%s, psta == NUL\n", __func__); + return false; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, + psta->state); + return false; + } + + 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 anyone sta is in ps mode */ + list_del_init(&pxmitframe->list); + + /* spin_lock_bh(&psta->sleep_q.lock); */ + + 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); + + /* DBG_8723A("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */ + + update_beacon23a(padapter, _TIM_IE_, NULL, false);/* tx bc/mc packets after upate bcn */ + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + + 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 & CHKBIT(psta->aid)) { + list_del_init(&pxmitframe->list); + + /* spin_lock_bh(&psta->sleep_q.lock); */ + + 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 |= CHKBIT(psta->aid); + + if (psta->sleepq_len == 1) { + /* upate BCN for TIM IE */ + update_beacon23a(padapter, _TIM_IE_, NULL, false); + } + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + + /* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */ + /* */ + /* wakeup_sta_to_xmit23a(padapter, psta); */ + /* */ + + ret = true; + + } + + } + + spin_unlock_bh(&psta->sleep_q.lock); + + return ret; +} + +static void +dequeue_xmitframes_to_sleeping_queue(struct rtw_adapter *padapter, + struct sta_info *psta, + struct rtw_queue *pframequeue) +{ + int ret; + struct list_head *plist, *phead, *ptmp; + 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); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, struct xmit_frame, list); + + ret = xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe); + + if (ret == true) { + pattrib = &pxmitframe->attrib; + + ptxservq = rtw_get_sta_pending23a(padapter, psta, pattrib->priority, (u8 *)(&ac_index)); + + ptxservq->qcnt--; + phwxmits[ac_index].accnt--; + } else { + /* DBG_8723A("xmitframe_enqueue_for_sleeping_sta23a return false\n"); */ + } + } +} + +void stop_sta_xmit23a(struct rtw_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_stainfo23a(padapter); + + spin_lock_bh(&pxmitpriv->lock); + + psta->state |= WIFI_SLEEP_STATE; + + pstapriv->sta_dz_bitmap |= CHKBIT(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_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta) +{ + u8 update_mask = 0, wmmps_ac = 0; + struct sta_info *psta_bmc; + struct list_head *plist, *phead, *ptmp; + struct xmit_frame *pxmitframe = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + spin_lock_bh(&pxmitpriv->lock); + + phead = get_list_head(&psta->sleep_q); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, struct xmit_frame, list); + 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; + rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe); + } + + if (psta->sleepq_len == 0) { + pstapriv->tim_bitmap &= ~CHKBIT(psta->aid); + + /* upate BCN for TIM IE */ + 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 &= ~CHKBIT(psta->aid); + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo23a(padapter); + if (!psta_bmc) + return; + + if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) { + /* no any sta in ps mode */ + spin_lock_bh(&pxmitpriv->lock); + + phead = get_list_head(&psta_bmc->sleep_q); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, struct xmit_frame, + list); + + 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; + rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe); + } + if (psta_bmc->sleepq_len == 0) { + pstapriv->tim_bitmap &= ~BIT(0); + pstapriv->sta_dz_bitmap &= ~BIT(0); + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_mask |= BIT(1); + } + + /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + } + + if (update_mask) + update_beacon23a(padapter, _TIM_IE_, NULL, false); +} + +void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter, + struct sta_info *psta) +{ + u8 wmmps_ac = 0; + struct list_head *plist, *phead, *ptmp; + struct xmit_frame *pxmitframe; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + /* spin_lock_bh(&psta->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + phead = get_list_head(&psta->sleep_q); + + list_for_each_safe(plist, ptmp, phead) { + pxmitframe = container_of(plist, struct xmit_frame, 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; + } + + 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; + + rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe); + + if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && + (wmmps_ac)) { + pstapriv->tim_bitmap &= ~CHKBIT(psta->aid); + + /* upate BCN for TIM IE */ + update_beacon23a(padapter, _TIM_IE_, NULL, false); + } + } + spin_unlock_bh(&pxmitpriv->lock); +} + +#endif + +void rtw_sctx_init23a(struct submit_ctx *sctx, int timeout_ms) +{ + sctx->timeout_ms = timeout_ms; + init_completion(&sctx->done); + sctx->status = RTW_SCTX_SUBMITTED; +} + +int rtw_sctx_wait23a(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_8723A("%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 rtw23a_sctx_done_err(struct submit_ctx **sctx, int status) +{ + if (*sctx) { + if (rtw_sctx_chk_waring_status(status)) + DBG_8723A("%s status:%d\n", __func__, status); + (*sctx)->status = status; + complete(&(*sctx)->done); + *sctx = NULL; + } +} + +void rtw_sctx_done23a(struct submit_ctx **sctx) +{ + rtw23a_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS); +} + +int rtw_ack_tx_wait23a(struct xmit_priv *pxmitpriv, u32 timeout_ms) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + pack_tx_ops->timeout_ms = timeout_ms; + pack_tx_ops->status = RTW_SCTX_SUBMITTED; + + return rtw_sctx_wait23a(pack_tx_ops); +} + +void rtw_ack_tx_done23a(struct xmit_priv *pxmitpriv, int status) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + if (pxmitpriv->ack_tx) + rtw23a_sctx_done_err(&pack_tx_ops, status); + else + DBG_8723A("%s ack_tx not set\n", __func__); +}