diff options
Diffstat (limited to 'drivers/staging/rtl8723bs/core')
19 files changed, 32897 insertions, 0 deletions
diff --git a/drivers/staging/rtl8723bs/core/rtw_ap.c b/drivers/staging/rtl8723bs/core/rtw_ap.c new file mode 100644 index 0000000..d3007c1 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_ap.c @@ -0,0 +1,2678 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> + +extern unsigned char RTW_WPA_OUI[]; +extern unsigned char WMM_OUI[]; +extern unsigned char WPS_OUI[]; +extern unsigned char P2P_OUI[]; +extern unsigned char WFD_OUI[]; + +void init_mlme_ap_info(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + + + spin_lock_init(&pmlmepriv->bcn_update_lock); + + /* for ACL */ + _rtw_init_queue(&pacl_list->acl_node_q); + + /* pmlmeext->bstart_bss = false; */ + + start_ap_mode(padapter); +} + +void free_mlme_ap_info(struct adapter *padapter) +{ + struct sta_info *psta = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* stop_ap_mode(padapter); */ + + pmlmepriv->update_bcn = false; + pmlmeext->bstart_bss = false; + + rtw_sta_flush(padapter); + + pmlmeinfo->state = _HW_STATE_NOLINK_; + + /* free_assoc_sta_resources */ + rtw_free_all_stainfo(padapter); + + /* free bc/mc sta_info */ + psta = rtw_get_bcmc_stainfo(padapter); + rtw_free_stainfo(padapter, psta); +} + +static void update_BCNTIM(struct adapter *padapter) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network); + unsigned char *pie = pnetwork_mlmeext->IEs; + + /* DBG_871X("%s\n", __func__); */ + + /* update TIM IE */ + /* if (pstapriv->tim_bitmap) */ + if (true) { + + u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL; + __le16 tim_bitmap_le; + uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen; + + tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap); + + p = rtw_get_ie( + pie + _FIXED_IE_LENGTH_, + _TIM_IE_, + &tim_ielen, + pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_ + ); + if (p != NULL && tim_ielen > 0) { + + tim_ielen += 2; + + premainder_ie = p+tim_ielen; + + tim_ie_offset = (sint)(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; + + /* calucate head_len */ + offset = _FIXED_IE_LENGTH_; + + /* get ssid_ie len */ + p = rtw_get_ie( + 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_ie( + pie + _BEACON_IE_OFFSET_, + _SUPPORTEDRATES_IE_, &tmp_len, + (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_) + ); + if (p != NULL) + offset += tmp_len+2; + + + /* DS Parameter Set IE, len =3 */ + offset += 3; + + premainder_ie = pie + offset; + + remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen; + + /* append TIM IE from offset */ + dst_ie = pie + offset; + + } + + + if (remainder_ielen > 0) { + + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie && premainder_ie) + memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + *dst_ie++ = _TIM_IE_; + + if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fe)) + 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) { + + __le16 pvb; + + if (pstapriv->tim_bitmap&0xff00) + pvb = cpu_to_le16(pstapriv->tim_bitmap >> 8); + else + pvb = tim_bitmap_le; + + *dst_ie++ = le16_to_cpu(pvb); + + } 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; + + } +} + +u8 chk_sta_is_alive(struct sta_info *psta); +u8 chk_sta_is_alive(struct sta_info *psta) +{ + #ifdef DBG_EXPIRATION_CHK + DBG_871X( + "sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n" + , MAC_ARG(psta->hwaddr) + , psta->rssi_stat.UndecoratedSmoothedPWDB + /* STA_RX_PKTS_ARG(psta) */ + , STA_RX_PKTS_DIFF_ARG(psta) + , psta->expire_to + , psta->state&WIFI_SLEEP_STATE?"PS, ":"" + , psta->state&WIFI_STA_ALIVE_CHK_STATE?"SAC, ":"" + , psta->sleepq_len + ); + #endif + + sta_update_last_rx_pkts(psta); + + return true; +} + +void expire_timeout_chk(struct adapter *padapter) +{ + struct list_head *phead, *plist; + u8 updated = false; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + + + spin_lock_bh(&pstapriv->auth_list_lock); + + phead = &pstapriv->auth_list; + plist = get_next(phead); + + /* check auth_queue */ + #ifdef DBG_EXPIRATION_CHK + if (phead != plist) { + DBG_871X(FUNC_NDEV_FMT" auth_list, cnt:%u\n" + , FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->auth_list_cnt); + } + #endif + while (phead != plist) { + + psta = LIST_CONTAINOR(plist, struct sta_info, auth_list); + + plist = get_next(plist); + + if (psta->expire_to > 0) { + + psta->expire_to--; + if (psta->expire_to == 0) { + + list_del_init(&psta->auth_list); + pstapriv->auth_list_cnt--; + + DBG_871X( + "auth expire %02X%02X%02X%02X%02X%02X\n", + psta->hwaddr[0], + psta->hwaddr[1], + psta->hwaddr[2], + psta->hwaddr[3], + psta->hwaddr[4], + psta->hwaddr[5] + ); + + spin_unlock_bh(&pstapriv->auth_list_lock); + + rtw_free_stainfo(padapter, psta); + + spin_lock_bh(&pstapriv->auth_list_lock); + } + } + + } + + spin_unlock_bh(&pstapriv->auth_list_lock); + psta = NULL; + + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* check asoc_queue */ + #ifdef DBG_EXPIRATION_CHK + if (phead != plist) { + DBG_871X(FUNC_NDEV_FMT" asoc_list, cnt:%u\n" + , FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->asoc_list_cnt); + } + #endif + while (phead != plist) { + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); +#ifdef CONFIG_AUTO_AP_MODE + if (psta->isrc) + continue; +#endif + if (chk_sta_is_alive(psta) || !psta->expire_to) { + psta->expire_to = pstapriv->expire_to; + psta->keep_alive_trycnt = 0; + psta->under_exist_checking = 0; + } else { + if (psta->expire_to > 0) + 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; + + /* DBG_871X("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->hwaddr)); */ + + /* to update bcn with tim_bitmap for this station */ + pstapriv->tim_bitmap |= BIT(psta->aid); + update_beacon(padapter, _TIM_IE_, NULL, true); + + if (!pmlmeext->active_keep_alive_check) + continue; + } + } + if (pmlmeext->active_keep_alive_check) { + int stainfo_offset; + + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + chk_alive_list[chk_alive_num++] = stainfo_offset; + + + continue; + } + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + DBG_871X( + "asoc expire "MAC_FMT", state = 0x%x\n", + MAC_ARG(psta->hwaddr), + psta->state + ); + updated = ap_free_sta(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_871X( + "%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_xmit(padapter, psta); + } + } + } + + spin_unlock_bh(&pstapriv->asoc_list_lock); + + if (chk_alive_num) { + u8 backup_oper_channel = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + /* switch to correct channel of current network before issue keep-alive frames */ + if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) { + backup_oper_channel = rtw_get_oper_ch(padapter); + SelectChannel(padapter, pmlmeext->cur_channel); + } + + /* issue null data to check sta alive*/ + for (i = 0; i < chk_alive_num; i++) { + int ret = _FAIL; + + psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]); + if (!(psta->state & _FW_LINKED)) + continue; + + if (psta->state & WIFI_SLEEP_STATE) + ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50); + else + ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50); + + psta->keep_alive_trycnt++; + if (ret == _SUCCESS) { + DBG_871X( + "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_871X( + "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_871X( + "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) == false) { + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + updated = ap_free_sta(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 */ + SelectChannel(padapter, backup_oper_channel); + } + + associated_clients_update(padapter, updated); +} + +void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level) +{ + unsigned char sta_band = 0, shortGIrate = false; + 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; + + if (!psta) + return; + + if (!(psta->state & _FW_LINKED)) + return; + + rtw_hal_update_sta_rate_mask(padapter, psta); + tx_ra_bitmap = psta->ra_mask; + + shortGIrate = query_ra_short_GI(psta); + + if (pcur_network->Configuration.DSConfig > 14) { + + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_5N; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11A; + } else { + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_24N; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11G; + + if (tx_ra_bitmap & 0x0f) + sta_band |= WIRELESS_11B; + } + + psta->wireless_mode = sta_band; + psta->raid = rtw_hal_networktype_to_raid(padapter, psta); + + if (psta->aid < NUM_STA) { + + u8 arg[4] = {0}; + + arg[0] = psta->mac_id; + arg[1] = psta->raid; + arg[2] = shortGIrate; + arg[3] = psta->init_rate; + + DBG_871X("%s => mac_id:%d , raid:%d , shortGIrate =%d, bitmap = 0x%x\n", + __func__, psta->mac_id, psta->raid, shortGIrate, tx_ra_bitmap); + + rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level); + } else{ + + + DBG_871X("station aid %d exceed the max number\n", psta->aid); + } + +} + +void update_bmc_sta(struct adapter *padapter) +{ + unsigned char network_type; + int supportRateNum = 0; + unsigned int tx_ra_bitmap = 0; + 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 + *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network; + struct sta_info *psta = rtw_get_bcmc_stainfo(padapter); + + if (psta) { + + psta->aid = 0;/* default set to 0 */ + /* psta->mac_id = psta->aid+4; */ + psta->mac_id = psta->aid + 1;/* mac_id = 1 for bc/mc stainfo */ + + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + psta->qos_option = 0; + psta->htpriv.ht_option = false; + + psta->ieee8021x_blocked = 0; + + memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + /* psta->dot118021XPrivacy = _NO_PRIVACY_;//!!! remove it, because it has been set before this. */ + + /* prepare for add_RATid */ + supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates); + network_type = rtw_check_network_type( + (u8 *)&pcur_network->SupportedRates, + supportRateNum, + pcur_network->Configuration.DSConfig + ); + if (IsSupportedTxCCK(network_type)) { + network_type = WIRELESS_11B; + } else if (network_type == WIRELESS_INVALID) { /* error handling */ + + if (pcur_network->Configuration.DSConfig > 14) + network_type = WIRELESS_11A; + else + network_type = WIRELESS_11B; + } + update_sta_basic_rate(psta, network_type); + psta->wireless_mode = network_type; + + rtw_hal_update_sta_rate_mask(padapter, psta); + tx_ra_bitmap = psta->ra_mask; + + psta->raid = rtw_hal_networktype_to_raid(padapter, psta); + + /* ap mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true); + + /* if (pHalData->fw_ractrl == true) */ + { + u8 arg[4] = {0}; + + arg[0] = psta->mac_id; + arg[1] = psta->raid; + arg[2] = 0; + arg[3] = psta->init_rate; + + DBG_871X("%s => mac_id:%d , raid:%d , bitmap = 0x%x\n", + __func__, psta->mac_id, psta->raid, tx_ra_bitmap); + + rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, 0); + } + + rtw_sta_media_status_rpt(padapter, psta, 1); + + spin_lock_bh(&psta->lock); + psta->state = _FW_LINKED; + spin_unlock_bh(&psta->lock); + + } else{ + + + DBG_871X("add_RATid_bmc_sta error!\n"); + } + +} + +/* notes: */ +/* AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */ +/* MAC_ID = AID+1 for sta in ap/adhoc mode */ +/* MAC_ID = 1 for bc/mc for sta/ap/adhoc */ +/* MAC_ID = 0 for bssid for sta/ap/adhoc */ +/* CAM_ID = 0~3 for default key, cmd_id =macid + 3, macid =aid+1; */ + +void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + struct ht_priv *phtpriv_sta = &psta->htpriv; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0; + /* set intf_tag to if1 */ + /* psta->intf_tag = 0; */ + + DBG_871X("%s\n", __func__); + + /* psta->mac_id = psta->aid+4; */ + /* psta->mac_id = psta->aid+1;//alloc macid when call rtw_alloc_stainfo(), */ + /* release macid when call rtw_free_stainfo() */ + + /* ap mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true); + + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + psta->ieee8021x_blocked = true; + else + psta->ieee8021x_blocked = false; + + + /* update sta's cap */ + + /* ERP */ + VCS_update(padapter, psta); + + /* HT related cap */ + if (phtpriv_sta->ht_option) { + + /* check if sta supports rx ampdu */ + phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable; + + phtpriv_sta->rx_ampdu_min_spacing = ( + phtpriv_sta->ht_cap.ampdu_params_info&IEEE80211_HT_CAP_AMPDU_DENSITY + )>>2; + + /* bwmode */ + if (( + phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info + ) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH)) + psta->bw_mode = CHANNEL_WIDTH_40; + else + psta->bw_mode = CHANNEL_WIDTH_20; + + if (pmlmeext->cur_bwmode < psta->bw_mode) + psta->bw_mode = pmlmeext->cur_bwmode; + + phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset; + + + /* check if sta support s Short GI 20M */ + if (( + phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info + ) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20)) + phtpriv_sta->sgi_20m = true; + + /* check if sta support s Short GI 40M */ + if (( + phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info + ) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) { + + if (psta->bw_mode == CHANNEL_WIDTH_40) /* according to psta->bw_mode */ + phtpriv_sta->sgi_40m = true; + else + phtpriv_sta->sgi_40m = false; + } + + psta->qos_option = true; + + /* B0 Config LDPC Coding Capability */ + if (TEST_FLAG(phtpriv_ap->ldpc_cap, LDPC_HT_ENABLE_TX) && + GET_HT_CAPABILITY_ELE_LDPC_CAP((u8 *)(&phtpriv_sta->ht_cap))) { + + SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX)); + DBG_871X("Enable HT Tx LDPC for STA(%d)\n", psta->aid); + } + + /* B7 B8 B9 Config STBC setting */ + if (TEST_FLAG(phtpriv_ap->stbc_cap, STBC_HT_ENABLE_TX) && + GET_HT_CAPABILITY_ELE_RX_STBC((u8 *)(&phtpriv_sta->ht_cap))) { + + SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX)); + DBG_871X("Enable HT Tx STBC for STA(%d)\n", psta->aid); + } + } else{ + + + phtpriv_sta->ampdu_enable = false; + + phtpriv_sta->sgi_20m = false; + phtpriv_sta->sgi_40m = false; + psta->bw_mode = CHANNEL_WIDTH_20; + phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + phtpriv_sta->ldpc_cap = cur_ldpc_cap; + phtpriv_sta->stbc_cap = cur_stbc_cap; + phtpriv_sta->beamform_cap = cur_beamform_cap; + + /* Rx AMPDU */ + send_delba(padapter, 0, psta->hwaddr);/* recipient */ + + /* TX AMPDU */ + send_delba(padapter, 1, psta->hwaddr);/* originator */ + phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */ + phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */ + + update_ldpc_stbc_cap(psta); + + /* todo: init other variables */ + + memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + + /* add ratid */ + /* add_RATid(padapter, psta);//move to ap_sta_info_defer_update() */ + + + spin_lock_bh(&psta->lock); + psta->state |= _FW_LINKED; + spin_unlock_bh(&psta->lock); + + +} + +static void update_ap_info(struct adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_bssid_ex + *pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + + psta->bssratelen = rtw_get_rateset_len(pnetwork->SupportedRates); + memcpy(psta->bssrateset, pnetwork->SupportedRates, psta->bssratelen); + + /* HT related cap */ + if (phtpriv_ap->ht_option) { + + /* check if sta supports rx ampdu */ + /* phtpriv_ap->ampdu_enable = phtpriv_ap->ampdu_enable; */ + + /* check if sta support s Short GI 20M */ + if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20)) + phtpriv_ap->sgi_20m = true; + + /* check if sta support s Short GI 40M */ + if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) + phtpriv_ap->sgi_40m = true; + + + psta->qos_option = true; + } else{ + + + phtpriv_ap->ampdu_enable = false; + + phtpriv_ap->sgi_20m = false; + phtpriv_ap->sgi_40m = false; + } + + psta->bw_mode = pmlmeext->cur_bwmode; + phtpriv_ap->ch_offset = pmlmeext->cur_ch_offset; + + phtpriv_ap->agg_enable_bitmap = 0x0;/* reset */ + phtpriv_ap->candidate_tid_bitmap = 0x0;/* reset */ + + memcpy(&psta->htpriv, &pmlmepriv->htpriv, sizeof(struct ht_priv)); +} + +static void update_hw_ht_param(struct adapter *padapter) +{ + unsigned char max_AMPDU_len; + unsigned char min_MPDU_spacing; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + DBG_871X("%s\n", __func__); + + + /* handle A-MPDU parameter field */ + /* + AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k + AMPDU_para [4:2]:Min MPDU Start Spacing + */ + max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03; + + min_MPDU_spacing = ( + pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c + ) >> 2; + + rtw_hal_set_hwreg( + padapter, + HW_VAR_AMPDU_MIN_SPACE, + (u8 *)(&min_MPDU_spacing) + ); + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); + + /* */ + /* Config SM Power Save setting */ + /* */ + pmlmeinfo->SM_PS = (le16_to_cpu( + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info + ) & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) + DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__); + + /* */ + /* Config current HT Protection mode. */ + /* */ + /* pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; */ + +} + +void start_bss_network(struct adapter *padapter, u8 *pbuf) +{ + u8 *p; + u8 val8, cur_channel, cur_bwmode, cur_ch_offset; + u16 bcn_interval; + u32 acparm; + int ie_len; + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &(padapter->securitypriv); + struct wlan_bssid_ex + *pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network); + struct HT_info_element *pht_info = NULL; + u8 cbw40_enable = 0; + + /* DBG_871X("%s\n", __func__); */ + + bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod; + cur_channel = pnetwork->Configuration.DSConfig; + cur_bwmode = 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_ie( + 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; + /* pmlmeinfo->HT_info_enable = true; */ + /* pmlmeinfo->HT_caps_enable = true; */ + + update_hw_ht_param(padapter); + } + + if (pmlmepriv->cur_network.join_res != true) { /* setting only at first time */ + + /* WEP Key will be set before this function, do not clear CAM. */ + if ( + (psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && + (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_) + ) + flush_all_cam_entry(padapter); /* clear CAM */ + } + + /* set MSR to AP_Mode */ + Set_MSR(padapter, _HW_STATE_AP_); + + /* Set BSSID REG */ + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->MacAddress); + + /* Set EDCA param reg */ + acparm = 0x002F3217; /* VO */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm)); + acparm = 0x005E4317; /* VI */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm)); + /* acparm = 0x00105320; // BE */ + acparm = 0x005ea42b; + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm)); + acparm = 0x0000A444; /* BK */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm)); + + /* Set Security */ + val8 = ( + psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X + ) ? 0xcc : 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* Beacon Control related register */ + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval)); + + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL); + + if (pmlmepriv->cur_network.join_res != true) { /* setting only at first time */ + + /* u32 initialgain; */ + + /* initialgain = 0x1e; */ + + + /* disable dynamic functions, such as high power, DIG */ + /* Save_DM_Func_Flag(padapter); */ + /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */ + + /* turn on all dynamic functions */ + Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); + + /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */ + + } + + /* set channel, bwmode */ + p = rtw_get_ie( + (pnetwork->IEs + sizeof(struct ndis_802_11_fix_ie)), + _HT_ADD_INFO_IE_, + &ie_len, + (pnetwork->IELength - sizeof(struct ndis_802_11_fix_ie)) + ); + if (p && ie_len) { + + pht_info = (struct HT_info_element *)(p+2); + + if (cur_channel > 14) { + if ((pregpriv->bw_mode & 0xf0) > 0) + cbw40_enable = 1; + } else { + if ((pregpriv->bw_mode & 0x0f) > 0) + cbw40_enable = 1; + } + + if ((cbw40_enable) && (pht_info->infos[0] & BIT(2))) { + + /* switch to the 40M Hz mode */ + /* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */ + cur_bwmode = 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: + /* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; */ + cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + /* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; */ + cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + + } + + } + + set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode); + DBG_871X( + "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; + + /* let pnetwork_mlmeext == pnetwork_mlme. */ + memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length); + + /* update cur_wireless_mode */ + update_wireless_mode(padapter); + + /* update RRSR after set channel and bandwidth */ + UpdateBrateTbl(padapter, pnetwork->SupportedRates); + rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates); + + /* udpate capability after cur_wireless_mode updated */ + update_capinfo( + padapter, + rtw_get_capability((struct wlan_bssid_ex *)pnetwork) + ); + + + if (true == pmlmeext->bstart_bss) { + + update_beacon(padapter, _TIM_IE_, NULL, true); + +#ifndef CONFIG_INTERRUPT_BASED_TXBCN /* other case will tx beacon when bcn interrupt coming in. */ + /* issue beacon frame */ + if (send_beacon(padapter) == _FAIL) + DBG_871X("issue_beacon, fail!\n"); + +#endif /* CONFIG_INTERRUPT_BASED_TXBCN */ + + } + + + /* update bc/mc sta_info */ + update_bmc_sta(padapter); + + /* pmlmeext->bstart_bss = true; */ + +} + +int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf, int len) +{ + int ret = _SUCCESS; + u8 *p; + u8 *pHT_caps_ie = NULL; + u8 *pHT_info_ie = NULL; + struct sta_info *psta = NULL; + u16 cap, ht_cap = false; + uint ie_len = 0; + int group_cipher, pairwise_cipher; + u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX]; + int supportRateNum = 0; + u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01}; + u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_bssid_ex + *pbss_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network; + u8 *ie = pbss_network->IEs; + + /* SSID */ + /* Supported rates */ + /* DS Params */ + /* WLAN_EID_COUNTRY */ + /* ERP Information element */ + /* Extended supported rates */ + /* WPA/WPA2 */ + /* Wi-Fi Wireless Multimedia Extensions */ + /* ht_capab, ht_oper */ + /* WPS IE */ + + DBG_871X("%s, len =%d\n", __func__, len); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true) + return _FAIL; + + + if (len > MAX_IE_SZ) + return _FAIL; + + pbss_network->IELength = len; + + memset(ie, 0, MAX_IE_SZ); + + memcpy(ie, pbuf, pbss_network->IELength); + + + if (pbss_network->InfrastructureMode != Ndis802_11APMode) + return _FAIL; + + pbss_network->Rssi = 0; + + memcpy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)), ETH_ALEN); + + /* beacon interval */ + p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8; 8: TimeStamp, 2: Beacon Interval 2:Capability */ + /* pbss_network->Configuration.BeaconPeriod = le16_to_cpu(*(unsigned short*)p); */ + pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p); + + /* capability */ + /* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */ + /* cap = le16_to_cpu(cap); */ + cap = RTW_GET_LE16(ie); + + /* SSID */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _SSID_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) { + + memset(&pbss_network->Ssid, 0, sizeof(struct ndis_802_11_ssid)); + memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len); + pbss_network->Ssid.SsidLength = ie_len; + } + + /* chnnel */ + channel = 0; + pbss_network->Configuration.Length = 0; + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _DSSET_IE_, &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) + channel = *(p + 2); + + pbss_network->Configuration.DSConfig = channel; + + + memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX); + /* get supported rates */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _SUPPORTEDRATES_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p != NULL) { + + memcpy(supportRate, p+2, ie_len); + supportRateNum = ie_len; + } + + /* get ext_supported rates */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _EXT_SUPPORTEDRATES_IE_, + &ie_len, + pbss_network->IELength - _BEACON_IE_OFFSET_ + ); + if (p != NULL) { + + memcpy(supportRate+supportRateNum, p+2, ie_len); + supportRateNum += ie_len; + + } + + network_type = rtw_check_network_type(supportRate, supportRateNum, channel); + + rtw_set_supported_rate(pbss_network->SupportedRates, network_type); + + + /* parsing ERP_IE */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _ERPINFO_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) + ERP_IE_handler(padapter, (struct ndis_80211_var_ie *)p); + + /* update privacy/security */ + if (cap & BIT(4)) + pbss_network->Privacy = 1; + else + pbss_network->Privacy = 0; + + psecuritypriv->wpa_psk = 0; + + /* wpa2 */ + group_cipher = 0; pairwise_cipher = 0; + psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_; + psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_; + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _RSN_IE_2_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) { + + if (rtw_parse_wpa2_ie( + p, + ie_len+2, + &group_cipher, + &pairwise_cipher, + NULL + ) == _SUCCESS) { + + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + + psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */ + psecuritypriv->wpa_psk |= BIT(1); + + psecuritypriv->wpa2_group_cipher = group_cipher; + psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher; + } + + } + + /* wpa */ + ie_len = 0; + group_cipher = 0; pairwise_cipher = 0; + psecuritypriv->wpa_group_cipher = _NO_PRIVACY_; + psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_; + for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) { + + p = rtw_get_ie( + p, + _SSN_IE_1_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)) + ); + if ((p) && (!memcmp(p+2, OUI1, 4))) { + + if (rtw_parse_wpa_ie( + p, + ie_len+2, + &group_cipher, + &pairwise_cipher, + NULL + ) == _SUCCESS) { + + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + + psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */ + + psecuritypriv->wpa_psk |= BIT(0); + + psecuritypriv->wpa_group_cipher = group_cipher; + psecuritypriv->wpa_pairwise_cipher = pairwise_cipher; + } + + break; + + } + + if ((p == NULL) || (ie_len == 0)) + break; + + + } + + /* wmm */ + ie_len = 0; + pmlmepriv->qospriv.qos_option = 0; + if (pregistrypriv->wmm_enable) { + + for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) { + + p = rtw_get_ie( + p, + _VENDOR_SPECIFIC_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)) + ); + if ((p) && !memcmp(p+2, WMM_PARA_IE, 6)) { + + pmlmepriv->qospriv.qos_option = 1; + + *(p+8) |= BIT(7);/* QoS Info, support U-APSD */ + + /* disable all ACM bits since the WMM admission control is not supported */ + *(p + 10) &= ~BIT(4); /* BE */ + *(p + 14) &= ~BIT(4); /* BK */ + *(p + 18) &= ~BIT(4); /* VI */ + *(p + 22) &= ~BIT(4); /* VO */ + + break; + } + + if ((p == NULL) || (ie_len == 0)) + break; + + } + } + + /* parsing HT_CAP_IE */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _HT_CAPABILITY_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) { + + u8 rf_type = 0; + u8 max_rx_ampdu_factor = 0; + struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_ieee80211_ht_cap *)(p+2); + + pHT_caps_ie = p; + + ht_cap = true; + network_type |= WIRELESS_11_24N; + + rtw_ht_use_default_setting(padapter); + + if (pmlmepriv->htpriv.sgi_20m == false) + pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_20)); + + if (pmlmepriv->htpriv.sgi_40m == false) + pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_40)); + + if (!TEST_FLAG(pmlmepriv->htpriv.ldpc_cap, LDPC_HT_ENABLE_RX)) + pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_LDPC_CODING)); + + + if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_TX)) + pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_TX_STBC)); + + + if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_RX)) + pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_RX_STBC_3R)); + + + pht_cap->ampdu_params_info &= ~( + IEEE80211_HT_CAP_AMPDU_FACTOR|IEEE80211_HT_CAP_AMPDU_DENSITY + ); + + if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) || + (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP)) { + + pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2)); + } else{ + + + pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00); + } + + rtw_hal_get_def_var( + padapter, + HW_VAR_MAX_RX_AMPDU_FACTOR, + &max_rx_ampdu_factor + ); + pht_cap->ampdu_params_info |= ( + IEEE80211_HT_CAP_AMPDU_FACTOR & max_rx_ampdu_factor + ); /* set Max Rx AMPDU size to 64K */ + + rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + if (rf_type == RF_1T1R) { + + pht_cap->supp_mcs_set[0] = 0xff; + pht_cap->supp_mcs_set[1] = 0x0; + } + + memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len); + + } + + /* parsing HT_INFO_IE */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _HT_ADD_INFO_IE_, + &ie_len, + (pbss_network->IELength - _BEACON_IE_OFFSET_) + ); + if (p && ie_len > 0) + pHT_info_ie = p; + + + switch (network_type) { + + case WIRELESS_11B: + pbss_network->NetworkTypeInUse = Ndis802_11DS; + break; + case WIRELESS_11G: + case WIRELESS_11BG: + case WIRELESS_11G_24N: + case WIRELESS_11BG_24N: + pbss_network->NetworkTypeInUse = Ndis802_11OFDM24; + break; + case WIRELESS_11A: + pbss_network->NetworkTypeInUse = Ndis802_11OFDM5; + break; + default: + pbss_network->NetworkTypeInUse = Ndis802_11OFDM24; + break; + } + + pmlmepriv->cur_network.network_type = network_type; + + pmlmepriv->htpriv.ht_option = false; + + if ((psecuritypriv->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) || + (psecuritypriv->wpa_pairwise_cipher&WPA_CIPHER_TKIP)) { + + /* todo: */ + /* ht_cap = false; */ + } + + /* ht_cap */ + if (pregistrypriv->ht_enable && ht_cap == true) { + + pmlmepriv->htpriv.ht_option = true; + pmlmepriv->qospriv.qos_option = 1; + + if (pregistrypriv->ampdu_enable == 1) + pmlmepriv->htpriv.ampdu_enable = true; + + + HT_caps_handler(padapter, (struct ndis_80211_var_ie *)pHT_caps_ie); + + HT_info_handler(padapter, (struct ndis_80211_var_ie *)pHT_info_ie); + } + + pbss_network->Length = get_wlan_bssid_ex_sz( + (struct wlan_bssid_ex *)pbss_network + ); + + /* issue beacon to start bss network */ + /* start_bss_network(padapter, (u8 *)pbss_network); */ + rtw_startbss_cmd(padapter, RTW_CMDF_WAIT_ACK); + + + /* alloc sta_info for ap itself */ + psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress); + if (!psta) { + + psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress); + if (psta == NULL) + return _FAIL; + + } + + /* update AP's sta info */ + update_ap_info(padapter, psta); + + psta->state |= WIFI_AP_STATE; /* Aries, add, fix bug of flush_cam_entry at STOP AP mode , 0724 */ + rtw_indicate_connect(padapter); + + pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */ + + /* update bc/mc sta_info */ + /* update_bmc_sta(padapter); */ + + return ret; + +} + +void rtw_set_macaddr_acl(struct adapter *padapter, int mode) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + + DBG_871X("%s, mode =%d\n", __func__, mode); + + pacl_list->mode = mode; +} + +int rtw_acl_add_sta(struct adapter *padapter, u8 *addr) +{ + struct list_head *plist, *phead; + u8 added = false; + int i, ret = 0; + struct rtw_wlan_acl_node *paclnode; + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + struct __queue *pacl_node_q = &pacl_list->acl_node_q; + + DBG_871X( + "%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); + plist = get_next(phead); + + while (phead != plist) { + + paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list); + plist = get_next(plist); + + if (!memcmp(paclnode->addr, addr, ETH_ALEN)) { + + if (paclnode->valid == true) { + + added = true; + DBG_871X("%s, sta has been added\n", __func__); + break; + } + } + } + + spin_unlock_bh(&(pacl_node_q->lock)); + + + if (added == true) + return ret; + + + spin_lock_bh(&(pacl_node_q->lock)); + + for (i = 0; i < NUM_ACL; i++) { + + paclnode = &pacl_list->aclnode[i]; + + if (paclnode->valid == false) { + + 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_871X("%s, acl_num =%d\n", __func__, pacl_list->num); + + spin_unlock_bh(&(pacl_node_q->lock)); + + return ret; +} + +int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr) +{ + struct list_head *plist, *phead; + int ret = 0; + struct rtw_wlan_acl_node *paclnode; + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + struct __queue *pacl_node_q = &pacl_list->acl_node_q; + u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; /* Baddr is used for clearing acl_list */ + + DBG_871X( + "%s(acl_num =%d) =" MAC_FMT "\n", + __func__, + pacl_list->num, + MAC_ARG(addr) + ); + + spin_lock_bh(&(pacl_node_q->lock)); + + phead = get_list_head(pacl_node_q); + plist = get_next(phead); + + while (phead != plist) { + + paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list); + plist = get_next(plist); + + if ( + !memcmp(paclnode->addr, addr, ETH_ALEN) || + !memcmp(baddr, addr, ETH_ALEN) + ) { + + if (paclnode->valid == true) { + + paclnode->valid = false; + + list_del_init(&paclnode->list); + + pacl_list->num--; + } + } + } + + spin_unlock_bh(&(pacl_node_q->lock)); + + DBG_871X("%s, acl_num =%d\n", __func__, pacl_list->num); + + return ret; + +} + +u8 rtw_ap_set_pairwise_key(struct adapter *padapter, struct sta_info *psta) +{ + struct cmd_obj *ph2c; + struct set_stakey_parm *psetstakey_para; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc( + sizeof(struct set_stakey_parm) + ); + if (psetstakey_para == NULL) { + kfree((u8 *) ph2c); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_); + + + psetstakey_para->algorithm = (u8)psta->dot118021XPrivacy; + + memcpy(psetstakey_para->addr, psta->hwaddr, ETH_ALEN); + + memcpy(psetstakey_para->key, &psta->dot118021x_UncstKey, 16); + + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +static int rtw_ap_set_key( + struct adapter *padapter, + u8 *key, + u8 alg, + int keyid, + u8 set_tx +) +{ + u8 keylen; + struct cmd_obj *pcmd; + struct setkey_parm *psetkeyparm; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + int res = _SUCCESS; + + /* DBG_871X("%s\n", __func__); */ + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm)); + if (psetkeyparm == NULL) { + kfree((unsigned char *)pcmd); + res = _FAIL; + goto exit; + } + + memset(psetkeyparm, 0, sizeof(struct setkey_parm)); + + psetkeyparm->keyid = (u8)keyid; + if (is_wep_enc(alg)) + padapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid); + + psetkeyparm->algorithm = alg; + + psetkeyparm->set_tx = set_tx; + + switch (alg) { + + case _WEP40_: + keylen = 5; + break; + case _WEP104_: + keylen = 13; + break; + case _TKIP_: + case _TKIP_WTMIC_: + case _AES_: + default: + keylen = 16; + } + + memcpy(&(psetkeyparm->key[0]), key, keylen); + + pcmd->cmdcode = _SetKey_CMD_; + pcmd->parmbuf = (u8 *)psetkeyparm; + pcmd->cmdsz = (sizeof(struct setkey_parm)); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + + INIT_LIST_HEAD(&pcmd->list); + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + + return res; +} + +int rtw_ap_set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid) +{ + DBG_871X("%s\n", __func__); + + return rtw_ap_set_key(padapter, key, alg, keyid, 1); +} + +int rtw_ap_set_wep_key( + struct adapter *padapter, + u8 *key, + u8 keylen, + int keyid, + u8 set_tx +) +{ + u8 alg; + + switch (keylen) { + + case 5: + alg = _WEP40_; + break; + case 13: + alg = _WEP104_; + break; + default: + alg = _NO_PRIVACY_; + } + + DBG_871X("%s\n", __func__); + + return rtw_ap_set_key(padapter, key, alg, keyid, set_tx); +} + +static void update_bcn_fixed_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_erpinfo_ie(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network); + unsigned char *p, *ie = pnetwork->IEs; + u32 len = 0; + + DBG_871X("%s, ERP_enable =%d\n", __func__, pmlmeinfo->ERP_enable); + + if (!pmlmeinfo->ERP_enable) + return; + + /* parsing ERP_IE */ + p = rtw_get_ie( + ie + _BEACON_IE_OFFSET_, + _ERPINFO_IE_, + &len, + (pnetwork->IELength - _BEACON_IE_OFFSET_) + ); + if (p && len > 0) { + + struct ndis_80211_var_ie *pIE = (struct ndis_80211_var_ie *)p; + + if (pmlmepriv->num_sta_non_erp == 1) + pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION; + else + pIE->data[0] &= ~( + RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION + ); + + if (pmlmepriv->num_sta_no_short_preamble > 0) + pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE; + else + pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE); + + ERP_IE_handler(padapter, pIE); + } + +} + +static void update_bcn_htcap_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_htinfo_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_rsn_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_wpa_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_wmm_ie(struct adapter *padapter) +{ + DBG_871X("%s\n", __func__); + +} + +static void update_bcn_wps_ie(struct adapter *padapter) +{ + u8 *pwps_ie = NULL; + u8 *pwps_ie_src; + u8 *premainder_ie; + u8 *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_871X("%s\n", __func__); + + pwps_ie = rtw_get_wps_ie( + ie+_FIXED_IE_LENGTH_, + ielen-_FIXED_IE_LENGTH_, + NULL, + &wps_ielen + ); + + if (pwps_ie == NULL || wps_ielen == 0) + return; + + pwps_ie_src = pmlmepriv->wps_beacon_ie; + if (pwps_ie_src == NULL) + return; + + wps_offset = (uint)(pwps_ie-ie); + + premainder_ie = pwps_ie + wps_ielen; + + remainder_ielen = ielen - wps_offset - wps_ielen; + + if (remainder_ielen > 0) { + + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie) + memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + 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; + } + + kfree(pbackup_remainder_ie); + + /* deal with the case without set_tx_beacon_cmd() in update_beacon() */ +#if defined(CONFIG_INTERRUPT_BASED_TXBCN) + if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) { + + u8 sr = 0; + + rtw_get_wps_attr_content( + pwps_ie_src, + wps_ielen, + WPS_ATTR_SELECTED_REGISTRAR, + (u8 *)(&sr), + NULL + ); + + if (sr) { + set_fwstate(pmlmepriv, WIFI_UNDER_WPS); + DBG_871X("%s, set WIFI_UNDER_WPS\n", __func__); + } + } +#endif +} + +static void update_bcn_p2p_ie(struct adapter *padapter) +{ + +} + +static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui) +{ + DBG_871X("%s\n", __func__); + + if (!memcmp(RTW_WPA_OUI, oui, 4)) + update_bcn_wpa_ie(padapter); + + else if (!memcmp(WMM_OUI, oui, 4)) + update_bcn_wmm_ie(padapter); + + else if (!memcmp(WPS_OUI, oui, 4)) + update_bcn_wps_ie(padapter); + + else if (!memcmp(P2P_OUI, oui, 4)) + update_bcn_p2p_ie(padapter); + + else + DBG_871X("unknown OUI type!\n"); + + + +} + +void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx) +{ + struct mlme_priv *pmlmepriv; + struct mlme_ext_priv *pmlmeext; + /* struct mlme_ext_info *pmlmeinfo; */ + + /* DBG_871X("%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); + +#ifndef CONFIG_INTERRUPT_BASED_TXBCN + if (tx) { + + /* send_beacon(padapter);//send_beacon must execute on TSR level */ + set_tx_beacon_cmd(padapter); + } +#endif /* CONFIG_INTERRUPT_BASED_TXBCN */ + +} + +/* +op_mode +Set to 0 (HT pure) under the followign conditions + - all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or + - all STAs in the BSS are 20 MHz HT in 20 MHz BSS +Set to 1 (HT non-member protection) if there may be non-HT STAs + in both the primary and the secondary channel +Set to 2 if only HT STAs are associated in BSS, + however and at least one 20 MHz HT STA is associated +Set to 3 (HT mixed mode) when one or more non-HT STAs are associated + (currently non-GF HT station is considered as non-HT STA also) +*/ +static int rtw_ht_operation_update(struct adapter *padapter) +{ + u16 cur_op_mode, new_op_mode; + int op_mode_changes = 0; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + + if (pmlmepriv->htpriv.ht_option == true) + return 0; + + /* if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed) */ + /* return 0; */ + + DBG_871X("%s current operation mode = 0x%X\n", + __func__, pmlmepriv->ht_op_mode); + + if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) + && pmlmepriv->num_sta_ht_no_gf) { + pmlmepriv->ht_op_mode |= + HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT; + op_mode_changes++; + } else if ((pmlmepriv->ht_op_mode & + HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) && + pmlmepriv->num_sta_ht_no_gf == 0) { + pmlmepriv->ht_op_mode &= + ~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT; + op_mode_changes++; + } + + if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) && + (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) { + pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT; + op_mode_changes++; + } else if ((pmlmepriv->ht_op_mode & + HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) && + (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) { + pmlmepriv->ht_op_mode &= + ~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT; + op_mode_changes++; + } + + /* Note: currently we switch to the MIXED op mode if HT non-greenfield + * station is associated. Probably it's a theoretical case, since + * it looks like all known HT STAs support greenfield. + */ + new_op_mode = 0; + if (pmlmepriv->num_sta_no_ht || + (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)) + new_op_mode = OP_MODE_MIXED; + else if ( + (le16_to_cpu(phtpriv_ap->ht_cap.cap_info) & IEEE80211_HT_CAP_SUP_WIDTH) + && pmlmepriv->num_sta_ht_20mhz) + new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED; + else if (pmlmepriv->olbc_ht) + new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS; + else + new_op_mode = OP_MODE_PURE; + + cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK; + if (cur_op_mode != new_op_mode) { + pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK; + pmlmepriv->ht_op_mode |= new_op_mode; + op_mode_changes++; + } + + DBG_871X("%s new operation mode = 0x%X changes =%d\n", + __func__, pmlmepriv->ht_op_mode, op_mode_changes); + + return op_mode_changes; + +} + +void associated_clients_update(struct adapter *padapter, u8 updated) +{ + /* update associcated stations cap. */ + if (updated == true) { + + struct list_head *phead, *plist; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* check asoc_queue */ + while (phead != plist) { + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + + plist = get_next(plist); + + VCS_update(padapter, psta); + } + + spin_unlock_bh(&pstapriv->asoc_list_lock); + + } + +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta) +{ + u8 beacon_updated = false; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) { + + if (!psta->no_short_preamble_set) { + + psta->no_short_preamble_set = 1; + + pmlmepriv->num_sta_no_short_preamble++; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_preamble == 1)) { + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + + } + } else{ + + + if (psta->no_short_preamble_set) { + + psta->no_short_preamble_set = 0; + + pmlmepriv->num_sta_no_short_preamble--; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_preamble == 0)) { + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + + } + } + + if (psta->flags & WLAN_STA_NONERP) { + + if (!psta->nonerp_set) { + + psta->nonerp_set = 1; + + pmlmepriv->num_sta_non_erp++; + + if (pmlmepriv->num_sta_non_erp == 1) { + + beacon_updated = true; + update_beacon(padapter, _ERPINFO_IE_, NULL, true); + } + } + + } else{ + + + if (psta->nonerp_set) { + + psta->nonerp_set = 0; + + pmlmepriv->num_sta_non_erp--; + + if (pmlmepriv->num_sta_non_erp == 0) { + + beacon_updated = true; + update_beacon(padapter, _ERPINFO_IE_, NULL, true); + } + } + + } + + + if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT)) { + + if (!psta->no_short_slot_time_set) { + + psta->no_short_slot_time_set = 1; + + pmlmepriv->num_sta_no_short_slot_time++; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_slot_time == 1)) { + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + + } + } else{ + + + if (psta->no_short_slot_time_set) { + + psta->no_short_slot_time_set = 0; + + pmlmepriv->num_sta_no_short_slot_time--; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_slot_time == 0)) { + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + } + } + + if (psta->flags & WLAN_STA_HT) { + + u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info); + + DBG_871X("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_871X("%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) == 0) { + if (!psta->ht_20mhz_set) { + psta->ht_20mhz_set = 1; + pmlmepriv->num_sta_ht_20mhz++; + } + DBG_871X("%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_871X("%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_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true); + } + + /* update associcated stations cap. */ + associated_clients_update(padapter, beacon_updated); + + DBG_871X("%s, updated =%d\n", __func__, beacon_updated); + +} + +u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta) +{ + u8 beacon_updated = false; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + if (!psta) + return beacon_updated; + + if (psta->no_short_preamble_set) { + psta->no_short_preamble_set = 0; + pmlmepriv->num_sta_no_short_preamble--; + if (pmlmeext->cur_wireless_mode > WIRELESS_11B + && pmlmepriv->num_sta_no_short_preamble == 0){ + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + } + + if (psta->nonerp_set) { + psta->nonerp_set = 0; + pmlmepriv->num_sta_non_erp--; + if (pmlmepriv->num_sta_non_erp == 0) { + + beacon_updated = true; + update_beacon(padapter, _ERPINFO_IE_, NULL, true); + } + } + + if (psta->no_short_slot_time_set) { + psta->no_short_slot_time_set = 0; + pmlmepriv->num_sta_no_short_slot_time--; + if (pmlmeext->cur_wireless_mode > WIRELESS_11B + && pmlmepriv->num_sta_no_short_slot_time == 0){ + + beacon_updated = true; + update_beacon(padapter, 0xFF, NULL, true); + } + } + + if (psta->no_ht_gf_set) { + psta->no_ht_gf_set = 0; + pmlmepriv->num_sta_ht_no_gf--; + } + + if (psta->no_ht_set) { + psta->no_ht_set = 0; + pmlmepriv->num_sta_no_ht--; + } + + if (psta->ht_20mhz_set) { + psta->ht_20mhz_set = 0; + pmlmepriv->num_sta_ht_20mhz--; + } + + if (rtw_ht_operation_update(padapter) > 0) { + + update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true); + } + + /* update associcated stations cap. */ + /* associated_clients_update(padapter, beacon_updated); //move it to avoid deadlock */ + + DBG_871X("%s, updated =%d\n", __func__, beacon_updated); + + return beacon_updated; + +} + +u8 ap_free_sta( + struct adapter *padapter, + struct sta_info *psta, + bool active, + u16 reason +) +{ + u8 beacon_updated = false; + + if (!psta) + return beacon_updated; + + if (active == true) { + + /* tear down Rx AMPDU */ + send_delba(padapter, 0, psta->hwaddr);/* recipient */ + + /* tear down TX AMPDU */ + send_delba(padapter, 1, psta->hwaddr);/* // originator */ + + issue_deauth(padapter, psta->hwaddr, reason); + } + + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + + + /* report_del_sta_event(padapter, psta->hwaddr, reason); */ + + /* clear cam entry / key */ + rtw_clearstakey_cmd(padapter, psta, 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_event(padapter, psta->hwaddr, reason); + + beacon_updated = bss_cap_update_on_sta_leave(padapter, psta); + + rtw_free_stainfo(padapter, psta); + + + return beacon_updated; + +} + +int rtw_sta_flush(struct adapter *padapter) +{ + struct list_head *phead, *plist; + int ret = 0; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev)); + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + return ret; + + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* free sta asoc_queue */ + while (phead != plist) { + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + + plist = get_next(plist); + + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + + /* spin_unlock_bh(&pstapriv->asoc_list_lock); */ + ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING); + /* spin_lock_bh(&pstapriv->asoc_list_lock); */ + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + + issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING); + + associated_clients_update(padapter, true); + + return ret; + +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void sta_info_update(struct adapter *padapter, struct sta_info *psta) +{ + int flags = psta->flags; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + + /* update wmm cap. */ + if (WLAN_STA_WME&flags) + psta->qos_option = 1; + else + psta->qos_option = 0; + + if (pmlmepriv->qospriv.qos_option == 0) + psta->qos_option = 0; + + /* update 802.11n ht cap. */ + if (WLAN_STA_HT&flags) { + + psta->htpriv.ht_option = true; + psta->qos_option = 1; + } else{ + + + psta->htpriv.ht_option = false; + } + + if (pmlmepriv->htpriv.ht_option == false) + psta->htpriv.ht_option = false; + + update_sta_info_apmode(padapter, psta); + + +} + +/* called >= TSR LEVEL for USB or SDIO Interface*/ +void ap_sta_info_defer_update(struct adapter *padapter, struct sta_info *psta) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (psta->state & _FW_LINKED) { + + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + /* add ratid */ + add_RATid(padapter, psta, 0);/* DM_RATR_STA_INIT */ + } +} +/* restore hw setting from sw data structures */ +void rtw_ap_restore_network(struct 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; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + + rtw_setopmode_cmd(padapter, Ndis802_11APMode, false); + + set_channel_bwmode( + 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_leave() */ + rtw_set_key( + padapter, + psecuritypriv, + psecuritypriv->dot118021XGrpKeyid, + 0, + false + ); + } + + spin_lock_bh(&pstapriv->asoc_list_lock); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + while (phead != plist) { + int stainfo_offset; + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + stainfo_offset = rtw_stainfo_offset(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_stainfo_by_offset(pstapriv, chk_alive_list[i]); + + if (psta == NULL) { + DBG_871X(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter)); + } else if (psta->state & _FW_LINKED) { + rtw_sta_media_status_rpt(padapter, psta, 1); + Update_RA_Entry(padapter, psta); + /* pairwise key */ + /* per sta pairwise key and settings */ + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + + rtw_setstakey_cmd(padapter, psta, true, false); + } + } + } + +} + +void start_ap_mode(struct adapter *padapter) +{ + int i; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + + pmlmepriv->update_bcn = false; + + /* init_mlme_ap_info(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; i < NUM_STA; i++) + pstapriv->sta_aid[i] = NULL; + + pmlmepriv->wps_beacon_ie = NULL; + pmlmepriv->wps_probe_resp_ie = NULL; + pmlmepriv->wps_assoc_resp_ie = NULL; + + pmlmepriv->p2p_beacon_ie = NULL; + pmlmepriv->p2p_probe_resp_ie = NULL; + + + /* for ACL */ + INIT_LIST_HEAD(&(pacl_list->acl_node_q.queue)); + pacl_list->num = 0; + pacl_list->mode = 0; + for (i = 0; i < NUM_ACL; i++) { + INIT_LIST_HEAD(&pacl_list->aclnode[i].list); + pacl_list->aclnode[i].valid = false; + } + +} + +void stop_ap_mode(struct adapter *padapter) +{ + struct list_head *phead, *plist; + struct rtw_wlan_acl_node *paclnode; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + struct __queue *pacl_node_q = &pacl_list->acl_node_q; + + pmlmepriv->update_bcn = false; + pmlmeext->bstart_bss = false; + + /* reset and init security priv , this can refine with rtw_reset_securitypriv */ + memset( + (unsigned char *)&padapter->securitypriv, + 0, + sizeof(struct security_priv) + ); + padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen; + padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled; + + /* for ACL */ + spin_lock_bh(&(pacl_node_q->lock)); + phead = get_list_head(pacl_node_q); + plist = get_next(phead); + while (phead != plist) { + + paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list); + plist = get_next(plist); + + if (paclnode->valid == true) { + + paclnode->valid = false; + + list_del_init(&paclnode->list); + + pacl_list->num--; + } + } + spin_unlock_bh(&(pacl_node_q->lock)); + + DBG_871X("%s, free acl_node_queue, num =%d\n", __func__, pacl_list->num); + + rtw_sta_flush(padapter); + + /* free_assoc_sta_resources */ + rtw_free_all_stainfo(padapter); + + psta = rtw_get_bcmc_stainfo(padapter); + rtw_free_stainfo(padapter, psta); + + rtw_init_bcmc_stainfo(padapter); + + rtw_free_mlme_priv_ie_data(pmlmepriv); + + rtw_btcoex_MediaStatusNotify(padapter, 0); /* disconnect */ +} diff --git a/drivers/staging/rtl8723bs/core/rtw_btcoex.c b/drivers/staging/rtl8723bs/core/rtw_btcoex.c new file mode 100644 index 0000000..3c5cb78 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_btcoex.c @@ -0,0 +1,243 @@ +/****************************************************************************** + * + * Copyright(c) 2013 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 <drv_types.h> +#include <rtw_debug.h> +#include <rtw_btcoex.h> +#include <hal_btcoex.h> + + +void rtw_btcoex_Initialize(struct adapter *padapter) +{ + hal_btcoex_Initialize(padapter); +} + +void rtw_btcoex_PowerOnSetting(struct adapter *padapter) +{ + hal_btcoex_PowerOnSetting(padapter); +} + +void rtw_btcoex_HAL_Initialize(struct adapter *padapter, u8 bWifiOnly) +{ + hal_btcoex_InitHwConfig(padapter, bWifiOnly); +} + +void rtw_btcoex_IpsNotify(struct adapter *padapter, u8 type) +{ + hal_btcoex_IpsNotify(padapter, type); +} + +void rtw_btcoex_LpsNotify(struct adapter *padapter, u8 type) +{ + hal_btcoex_LpsNotify(padapter, type); +} + +void rtw_btcoex_ScanNotify(struct adapter *padapter, u8 type) +{ + hal_btcoex_ScanNotify(padapter, type); +} + +void rtw_btcoex_ConnectNotify(struct adapter *padapter, u8 action) +{ + hal_btcoex_ConnectNotify(padapter, action); +} + +void rtw_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus) +{ + if ((RT_MEDIA_CONNECT == mediaStatus) + && (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)) { + rtw_hal_set_hwreg(padapter, HW_VAR_DL_RSVD_PAGE, NULL); + } + + hal_btcoex_MediaStatusNotify(padapter, mediaStatus); +} + +void rtw_btcoex_SpecialPacketNotify(struct adapter *padapter, u8 pktType) +{ + hal_btcoex_SpecialPacketNotify(padapter, pktType); +} + +void rtw_btcoex_IQKNotify(struct adapter *padapter, u8 state) +{ + hal_btcoex_IQKNotify(padapter, state); +} + +void rtw_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf) +{ + hal_btcoex_BtInfoNotify(padapter, length, tmpBuf); +} + +void rtw_btcoex_SuspendNotify(struct adapter *padapter, u8 state) +{ + hal_btcoex_SuspendNotify(padapter, state); +} + +void rtw_btcoex_HaltNotify(struct adapter *padapter) +{ + if (false == padapter->bup) { + DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n", + FUNC_ADPT_ARG(padapter), padapter->bup); + + return; + } + + if (true == padapter->bSurpriseRemoved) { + DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n", + FUNC_ADPT_ARG(padapter), padapter->bSurpriseRemoved); + + return; + } + + hal_btcoex_HaltNotify(padapter); +} + +u8 rtw_btcoex_IsBtDisabled(struct adapter *padapter) +{ + return hal_btcoex_IsBtDisabled(padapter); +} + +void rtw_btcoex_Handler(struct adapter *padapter) +{ + hal_btcoex_Hanlder(padapter); +} + +s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter) +{ + s32 coexctrl; + + coexctrl = hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter); + + return coexctrl; +} + +void rtw_btcoex_SetManualControl(struct adapter *padapter, u8 manual) +{ + if (true == manual) { + hal_btcoex_SetManualControl(padapter, true); + } else{ + hal_btcoex_SetManualControl(padapter, false); + } +} + +u8 rtw_btcoex_IsBtControlLps(struct adapter *padapter) +{ + return hal_btcoex_IsBtControlLps(padapter); +} + +u8 rtw_btcoex_IsLpsOn(struct adapter *padapter) +{ + return hal_btcoex_IsLpsOn(padapter); +} + +u8 rtw_btcoex_RpwmVal(struct adapter *padapter) +{ + return hal_btcoex_RpwmVal(padapter); +} + +u8 rtw_btcoex_LpsVal(struct adapter *padapter) +{ + return hal_btcoex_LpsVal(padapter); +} + +void rtw_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist) +{ + hal_btcoex_SetBTCoexist(padapter, bBtExist); +} + +void rtw_btcoex_SetChipType(struct adapter *padapter, u8 chipType) +{ + hal_btcoex_SetChipType(padapter, chipType); +} + +void rtw_btcoex_SetPGAntNum(struct adapter *padapter, u8 antNum) +{ + hal_btcoex_SetPgAntNum(padapter, antNum); +} + +void rtw_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath) +{ + hal_btcoex_SetSingleAntPath(padapter, singleAntPath); +} + +u32 rtw_btcoex_GetRaMask(struct adapter *padapter) +{ + return hal_btcoex_GetRaMask(padapter); +} + +void rtw_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen) +{ + hal_btcoex_RecordPwrMode(padapter, pCmdBuf, cmdLen); +} + +void rtw_btcoex_DisplayBtCoexInfo(struct adapter *padapter, u8 *pbuf, u32 bufsize) +{ + hal_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize); +} + +void rtw_btcoex_SetDBG(struct adapter *padapter, u32 *pDbgModule) +{ + hal_btcoex_SetDBG(padapter, pDbgModule); +} + +u32 rtw_btcoex_GetDBG(struct adapter *padapter, u8 *pStrBuf, u32 bufSize) +{ + return hal_btcoex_GetDBG(padapter, pStrBuf, bufSize); +} + +/* ================================================== */ +/* Below Functions are called by BT-Coex */ +/* ================================================== */ +void rtw_btcoex_RejectApAggregatedPacket(struct adapter *padapter, u8 enable) +{ + struct mlme_ext_info *pmlmeinfo; + struct sta_info *psta; + + pmlmeinfo = &padapter->mlmeextpriv.mlmext_info; + psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv)); + + if (true == enable) { + pmlmeinfo->bAcceptAddbaReq = false; + if (psta) + send_delba(padapter, 0, psta->hwaddr); + } else{ + pmlmeinfo->bAcceptAddbaReq = true; + } +} + +void rtw_btcoex_LPS_Enter(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv; + u8 lpsVal; + + + pwrpriv = adapter_to_pwrctl(padapter); + + pwrpriv->bpower_saving = true; + lpsVal = rtw_btcoex_LpsVal(padapter); + rtw_set_ps_mode(padapter, PS_MODE_MIN, 0, lpsVal, "BTCOEX"); +} + +void rtw_btcoex_LPS_Leave(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv; + + + pwrpriv = adapter_to_pwrctl(padapter); + + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "BTCOEX"); + LPS_RF_ON_check(padapter, 100); + pwrpriv->bpower_saving = false; + } +} diff --git a/drivers/staging/rtl8723bs/core/rtw_cmd.c b/drivers/staging/rtl8723bs/core/rtw_cmd.c new file mode 100644 index 0000000..080c81b --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_cmd.c @@ -0,0 +1,2226 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <linux/jiffies.h> + +static 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_callback}, + {GEN_CMD_CODE(_Write_BBREG), NULL}, + {GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback}, + {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_cmd_callback}, /*14*/ + {GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd_callback}, /*15*/ + {GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd_callback}, + {GEN_CMD_CODE(_SetOpMode), NULL}, + {GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback}, /*18*/ + {GEN_CMD_CODE(_SetAuth), NULL}, + + {GEN_CMD_CODE(_SetKey), NULL}, /*20*/ + {GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback}, + {GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback}, + {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*/ + {GEN_CMD_CODE(_ChkBMCSleepq), NULL}, /*63*/ + + {GEN_CMD_CODE(_RunInThreadCMD), NULL},/*64*/ +}; + +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 joinbss_parm), join_cmd_hdl) /*14*/ + GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl) + GEN_MLME_EXT_HANDLER(sizeof(struct createbss_parm), createbss_hdl) + GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl) + GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl) /*18*/ + GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl) + GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl) /*20*/ + GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl) + 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_hdl) + GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl) /* 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_hdl) /*55*/ + + GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl) /*56*/ + GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl) /*57*/ + + GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl) /*58*/ + GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param), set_chplan_hdl) /*59*/ + GEN_MLME_EXT_HANDLER(sizeof(struct LedBlink_param), led_blink_hdl) /*60*/ + + GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param), set_csa_hdl) /*61*/ + GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param), tdls_hdl) /*62*/ + GEN_MLME_EXT_HANDLER(0, chk_bmc_sleepq_hdl) /*63*/ + GEN_MLME_EXT_HANDLER(sizeof(struct RunInThread_param), run_in_thread_hdl) /*63*/ +}; + +/* +Caller and the rtw_cmd_thread can protect cmd_q by spin_lock. +No irqsave is necessary. +*/ + +sint _rtw_init_cmd_priv(struct cmd_priv *pcmdpriv) +{ + sint res = _SUCCESS; + + sema_init(&(pcmdpriv->cmd_queue_sema), 0); + /* sema_init(&(pcmdpriv->cmd_done_sema), 0); */ + sema_init(&(pcmdpriv->terminate_cmdthread_sema), 0); + + + _rtw_init_queue(&(pcmdpriv->cmd_queue)); + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + + pcmdpriv->cmd_seq = 1; + + pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ); + + if (pcmdpriv->cmd_allocated_buf == NULL) { + res = _FAIL; + goto exit; + } + + pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf + CMDBUFF_ALIGN_SZ - ((SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1)); + + pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4); + + if (pcmdpriv->rsp_allocated_buf == NULL) { + res = _FAIL; + goto exit; + } + + pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3); + + pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0; + + mutex_init(&pcmdpriv->sctx_mutex); +exit: + return res; +} + +static void c2h_wk_callback(_workitem *work); +sint _rtw_init_evt_priv(struct evt_priv *pevtpriv) +{ + sint res = _SUCCESS; + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + atomic_set(&pevtpriv->event_seq, 0); + pevtpriv->evt_done_cnt = 0; + + _init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL); + pevtpriv->c2h_wk_alive = false; + pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1); + + return res; +} + +void _rtw_free_evt_priv(struct evt_priv *pevtpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+_rtw_free_evt_priv\n")); + + _cancel_workitem_sync(&pevtpriv->c2h_wk); + while (pevtpriv->c2h_wk_alive) + msleep(10); + + while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) { + void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue); + if (c2h != NULL && c2h != (void *)pevtpriv) { + kfree(c2h); + } + } + kfree(pevtpriv->c2h_queue); + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("-_rtw_free_evt_priv\n")); +} + +void _rtw_free_cmd_priv(struct cmd_priv *pcmdpriv) +{ + if (pcmdpriv) { + kfree(pcmdpriv->cmd_allocated_buf); + + kfree(pcmdpriv->rsp_allocated_buf); + + mutex_destroy(&pcmdpriv->sctx_mutex); + } +} + +/* +Calling Context: + +rtw_enqueue_cmd can only be called between kernel thread, +since only spin_lock is used. + +ISR/Call-Back functions can't call this sub-function. + +*/ + +sint _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj) +{ + _irqL irqL; + + if (obj == NULL) + goto exit; + + /* spin_lock_bh(&queue->lock); */ + spin_lock_irqsave(&queue->lock, irqL); + + list_add_tail(&obj->list, &queue->queue); + + /* spin_unlock_bh(&queue->lock); */ + spin_unlock_irqrestore(&queue->lock, irqL); + +exit: + return _SUCCESS; +} + +struct cmd_obj *_rtw_dequeue_cmd(struct __queue *queue) +{ + _irqL irqL; + struct cmd_obj *obj; + + /* spin_lock_bh(&(queue->lock)); */ + spin_lock_irqsave(&queue->lock, irqL); + if (list_empty(&(queue->queue))) + obj = NULL; + else{ + obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list); + list_del_init(&obj->list); + } + + /* spin_unlock_bh(&(queue->lock)); */ + spin_unlock_irqrestore(&queue->lock, irqL); + + return obj; +} + +u32 rtw_init_cmd_priv(struct cmd_priv *pcmdpriv) +{ + u32 res; + + res = _rtw_init_cmd_priv(pcmdpriv); + return res; +} + +u32 rtw_init_evt_priv(struct evt_priv *pevtpriv) +{ + int res; + + res = _rtw_init_evt_priv(pevtpriv); + return res; +} + +void rtw_free_evt_priv(struct evt_priv *pevtpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_evt_priv\n")); + _rtw_free_evt_priv(pevtpriv); +} + +void rtw_free_cmd_priv(struct cmd_priv *pcmdpriv) +{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_cmd_priv\n")); + _rtw_free_cmd_priv(pcmdpriv); +} + +int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj); +int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ + u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */ + + if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan)) + bAllow = true; + + if ((pcmdpriv->padapter->hw_init_completed == false && bAllow == false) + || atomic_read(&(pcmdpriv->cmdthd_running)) == false /* com_thread not running */ + ) { + /* DBG_871X("%s:%s: drop cmdcode:%u, hw_init_completed:%u, cmdthd_running:%u\n", caller_func, __func__, */ + /* cmd_obj->cmdcode, */ + /* pcmdpriv->padapter->hw_init_completed, */ + /* pcmdpriv->cmdthd_running */ + /* */ + + return _FAIL; + } + return _SUCCESS; +} + + + +u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ + int res = _FAIL; + struct adapter *padapter = pcmdpriv->padapter; + + if (cmd_obj == NULL) { + goto exit; + } + + cmd_obj->padapter = padapter; + + res = rtw_cmd_filter(pcmdpriv, cmd_obj); + if (_FAIL == res) { + rtw_free_cmd_obj(cmd_obj); + goto exit; + } + + res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj); + + if (res == _SUCCESS) + up(&pcmdpriv->cmd_queue_sema); + +exit: + return res; +} + +struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv) +{ + struct cmd_obj *cmd_obj; + + cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue); + + return cmd_obj; +} + +void rtw_free_cmd_obj(struct cmd_obj *pcmd) +{ + if ((pcmd->cmdcode != _JoinBss_CMD_) && + (pcmd->cmdcode != _CreateBss_CMD_)) { + /* free parmbuf in cmd_obj */ + kfree((unsigned char *)pcmd->parmbuf); + } + + if (pcmd->rsp != NULL) { + if (pcmd->rspsz != 0) { + /* free rsp in cmd_obj */ + kfree((unsigned char *)pcmd->rsp); + } + } + + /* free cmd_obj */ + kfree((unsigned char *)pcmd); +} + + +void rtw_stop_cmd_thread(struct adapter *adapter) +{ + if (adapter->cmdThread && + atomic_read(&(adapter->cmdpriv.cmdthd_running)) == true && + adapter->cmdpriv.stop_req == 0) { + adapter->cmdpriv.stop_req = 1; + up(&adapter->cmdpriv.cmd_queue_sema); + down(&adapter->cmdpriv.terminate_cmdthread_sema); + } +} + +int rtw_cmd_thread(void *context) +{ + u8 ret; + struct cmd_obj *pcmd; + u8 *pcmdbuf, *prspbuf; + unsigned long cmd_start_time; + unsigned long cmd_process_time; + u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf); + void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd); + struct adapter *padapter = (struct adapter *)context; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + struct drvextra_cmd_parm *extra_parm = NULL; + + thread_enter("RTW_CMD_THREAD"); + + pcmdbuf = pcmdpriv->cmd_buf; + prspbuf = pcmdpriv->rsp_buf; + + pcmdpriv->stop_req = 0; + atomic_set(&(pcmdpriv->cmdthd_running), true); + up(&pcmdpriv->terminate_cmdthread_sema); + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n")); + + while (1) { + if (down_interruptible(&pcmdpriv->cmd_queue_sema)) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" down_interruptible(&pcmdpriv->cmd_queue_sema) return != 0, break\n", FUNC_ADPT_ARG(padapter)); + break; + } + + if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) { + DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n", + __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__); + break; + } + + if (pcmdpriv->stop_req) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" stop_req:%u, break\n", FUNC_ADPT_ARG(padapter), pcmdpriv->stop_req); + break; + } + + if (list_empty(&(pcmdpriv->cmd_queue.queue))) { + /* DBG_871X("%s: cmd queue is empty!\n", __func__); */ + continue; + } + + if (rtw_register_cmd_alive(padapter) != _SUCCESS) { + RT_TRACE(_module_hal_xmit_c_, _drv_notice_, + ("%s: wait to leave LPS_LCLK\n", __func__)); + continue; + } + +_next: + if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) { + DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n", + __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__); + break; + } + + pcmd = rtw_dequeue_cmd(pcmdpriv); + if (!pcmd) { + rtw_unregister_cmd_alive(padapter); + continue; + } + + cmd_start_time = jiffies; + + if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) { + pcmd->res = H2C_DROPPED; + goto post_process; + } + + pcmdpriv->cmd_issued_cnt++; + + pcmd->cmdsz = _RND4((pcmd->cmdsz));/* _RND4 */ + + memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz); + + if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) { + cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns; + + if (cmd_hdl) { + ret = cmd_hdl(pcmd->padapter, pcmdbuf); + pcmd->res = ret; + } + + pcmdpriv->cmd_seq++; + } else{ + pcmd->res = H2C_PARAMETERS_ERROR; + } + + cmd_hdl = NULL; + +post_process: + + if (mutex_lock_interruptible(&(pcmd->padapter->cmdpriv.sctx_mutex)) == 0) { + if (pcmd->sctx) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" pcmd->sctx\n", + FUNC_ADPT_ARG(pcmd->padapter)); + + if (pcmd->res == H2C_SUCCESS) + rtw_sctx_done(&pcmd->sctx); + else + rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_ERROR); + } + mutex_unlock(&(pcmd->padapter->cmdpriv.sctx_mutex)); + } + + cmd_process_time = jiffies_to_msecs(jiffies - cmd_start_time); + if (cmd_process_time > 1000) { + if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) { + DBG_871X(ADPT_FMT" cmd =%d process_time =%lu > 1 sec\n", + ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time); + /* rtw_warn_on(1); */ + } else if (pcmd->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)) { + DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n", + ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time); + /* rtw_warn_on(1); */ + } else { + DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n", + ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time); + /* rtw_warn_on(1); */ + } + } + + /* call callback function for post-processed */ + if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) { + pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback; + if (pcmd_callback == NULL) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode)); + rtw_free_cmd_obj(pcmd); + } else{ + /* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */ + pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */ + } + } else{ + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode)); + rtw_free_cmd_obj(pcmd); + } + + flush_signals_thread(); + + goto _next; + + } + + /* free all cmd_obj resources */ + do { + pcmd = rtw_dequeue_cmd(pcmdpriv); + if (pcmd == NULL) { + rtw_unregister_cmd_alive(padapter); + break; + } + + /* DBG_871X("%s: leaving... drop cmdcode:%u size:%d\n", __func__, pcmd->cmdcode, pcmd->cmdsz); */ + + if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) { + extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf; + if (extra_parm->pbuf && extra_parm->size > 0) { + kfree(extra_parm->pbuf); + } + } + + rtw_free_cmd_obj(pcmd); + } while (1); + + up(&pcmdpriv->terminate_cmdthread_sema); + atomic_set(&(pcmdpriv->cmdthd_running), false); + + thread_exit(); +} + +/* +rtw_sitesurvey_cmd(~) + ### NOTE:#### (!!!!) + MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock +*/ +u8 rtw_sitesurvey_cmd(struct adapter *padapter, struct ndis_802_11_ssid *ssid, int ssid_num, + struct rtw_ieee80211_channel *ch, int ch_num) +{ + u8 res = _FAIL; + struct cmd_obj *ph2c; + struct sitesurvey_parm *psurveyPara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1); + } + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) + return _FAIL; + + psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm)); + if (psurveyPara == NULL) { + kfree((unsigned char *) ph2c); + return _FAIL; + } + + rtw_free_network_queue(padapter, false); + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__)); + + init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey)); + + /* psurveyPara->bsslimit = 48; */ + psurveyPara->scan_mode = pmlmepriv->scan_mode; + + /* prepare ssid list */ + if (ssid) { + int i; + for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) { + if (ssid[i].SsidLength) { + memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid)); + psurveyPara->ssid_num++; + + DBG_871X(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter), + psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength); + } + } + } + + /* prepare channel list */ + if (ch) { + int i; + for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) { + if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) { + memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel)); + psurveyPara->ch_num++; + + DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), + psurveyPara->ch[i].hw_value); + } + } + } + + set_fwstate(pmlmepriv, _FW_UNDER_SURVEY); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + if (res == _SUCCESS) { + + pmlmepriv->scan_start_time = jiffies; + _set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT); + } else { + _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY); + } + return res; +} + +u8 rtw_setdatarate_cmd(struct adapter *padapter, u8 *rateset) +{ + struct cmd_obj *ph2c; + struct setdatarate_parm *pbsetdataratepara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pbsetdataratepara = (struct setdatarate_parm *)rtw_zmalloc(sizeof(struct setdatarate_parm)); + if (pbsetdataratepara == NULL) { + kfree((u8 *) ph2c); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate)); + pbsetdataratepara->mac_id = 5; + memcpy(pbsetdataratepara->datarates, rateset, NumRates); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); +exit: + return res; +} + +void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + /* rtw_free_cmd_obj(pcmd); */ + kfree((unsigned char *) pcmd->parmbuf); + kfree((unsigned char *) pcmd); +} + +u8 rtw_createbss_cmd(struct adapter *padapter) +{ + struct cmd_obj *pcmd; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network; + u8 res = _SUCCESS; + + if (pmlmepriv->assoc_ssid.SsidLength == 0) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid)); + } else { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid)); + } + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + + INIT_LIST_HEAD(&pcmd->list); + pcmd->cmdcode = _CreateBss_CMD_; + pcmd->parmbuf = (unsigned char *)pdev_network; + pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + pdev_network->Length = pcmd->cmdsz; + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + return res; +} + +u8 rtw_startbss_cmd(struct adapter *padapter, int flags) +{ + struct cmd_obj *pcmd; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + start_bss_network(padapter, (u8 *)&(padapter->mlmepriv.cur_network.network)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + + INIT_LIST_HEAD(&pcmd->list); + pcmd->cmdcode = GEN_CMD_CODE(_CreateBss); + pcmd->parmbuf = NULL; + pcmd->cmdsz = 0; + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + if (flags & RTW_CMDF_WAIT_ACK) { + pcmd->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + if (mutex_lock_interruptible(&pcmdpriv->sctx_mutex) == 0) { + if (sctx.status == RTW_SCTX_SUBMITTED) + pcmd->sctx = NULL; + mutex_unlock(&pcmdpriv->sctx_mutex); + } + } + } + +exit: + return res; +} + +u8 rtw_joinbss_cmd(struct 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_NETWORK_INFRASTRUCTURE ndis_network_mode = pnetwork->network.InfrastructureMode; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u32 tmp_len; + u8 *ptmp = NULL; + + if (pmlmepriv->assoc_ssid.SsidLength == 0) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n")); + } else { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid)); + } + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n")); + goto exit; + } + /* for IEs is fix buf size */ + t_len = sizeof(struct wlan_bssid_ex); + + + /* for hidden ap to set fw_state here */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) != true) { + switch (ndis_network_mode) { + case Ndis802_11IBSS: + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + break; + + case Ndis802_11Infrastructure: + set_fwstate(pmlmepriv, WIFI_STATION_STATE); + break; + + case Ndis802_11APMode: + case Ndis802_11AutoUnknown: + case Ndis802_11InfrastructureMax: + break; + + } + } + + psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss; + if (psecnetwork == NULL) { + if (pcmd != NULL) + kfree((unsigned char *)pcmd); + + res = _FAIL; + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n")); + + goto exit; + } + + memset(psecnetwork, 0, t_len); + + memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network)); + + 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) { + memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN); + } + + psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength); + + + pqospriv->qos_option = 0; + + if (pregistrypriv->wmm_enable) { + tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength); + + if (psecnetwork->IELength != tmp_len) { + psecnetwork->IELength = tmp_len; + pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */ + } else{ + pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */ + } + } + + phtpriv->ht_option = false; + ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &tmp_len, pnetwork->network.IELength-12); + if (pregistrypriv->ht_enable && ptmp && tmp_len > 0) { + /* 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_ht_use_default_setting(padapter); + + rtw_build_wmm_ie_ht(padapter, &psecnetwork->IEs[12], &psecnetwork->IELength); + + /* rtw_restructure_ht_ie */ + rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[12], &psecnetwork->IEs[0], + pnetwork->network.IELength-12, &psecnetwork->IELength, + pnetwork->network.Configuration.DSConfig); + } + } + + rtw_append_exented_cap(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength); + + pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength); + + pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */ + + INIT_LIST_HEAD(&pcmd->list); + pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */ + pcmd->parmbuf = (unsigned char *)psecnetwork; + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + return res; +} + +u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */ +{ + struct cmd_obj *cmdobj = NULL; + struct disconnect_parm *param = NULL; + struct cmd_priv *cmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n")); + + /* prepare cmd parameter */ + param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param)); + if (param == NULL) { + res = _FAIL; + goto exit; + } + param->deauth_timeout_ms = deauth_timeout_ms; + + if (enqueue) { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + kfree((u8 *)param); + goto exit; + } + init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_); + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + } else { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param)) + res = _FAIL; + kfree((u8 *)param); + } + +exit: + return res; +} + +u8 rtw_setopmode_cmd(struct adapter *padapter, enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype, bool enqueue) +{ + struct cmd_obj *ph2c; + struct setopmode_parm *psetop; + + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + psetop = (struct setopmode_parm *)rtw_zmalloc(sizeof(struct setopmode_parm)); + + if (psetop == NULL) { + res = _FAIL; + goto exit; + } + psetop->mode = (u8)networktype; + + if (enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + kfree((u8 *)psetop); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_); + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else{ + setopmode_hdl(padapter, (u8 *)psetop); + kfree((u8 *)psetop); + } +exit: + return res; +} + +u8 rtw_setstakey_cmd(struct adapter *padapter, struct sta_info *sta, u8 unicast_key, bool 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 mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + u8 res = _SUCCESS; + + psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm)); + if (psetstakey_para == NULL) { + res = _FAIL; + goto exit; + } + + memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm; + } else { + GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false); + } + + if (unicast_key == true) { + memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16); + } else{ + memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16); + } + + /* jeff: set this becasue at least sw key is ready */ + padapter->securitypriv.busetkipkey = true; + + if (enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + kfree((u8 *) psetstakey_para); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp)); + if (psetstakey_rsp == NULL) { + kfree((u8 *) ph2c); + kfree((u8 *) 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); + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else{ + set_stakey_hdl(padapter, (u8 *)psetstakey_para); + kfree((u8 *) psetstakey_para); + } +exit: + return res; +} + +u8 rtw_clearstakey_cmd(struct adapter *padapter, struct sta_info *sta, 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; + s16 cam_id = 0; + u8 res = _SUCCESS; + + if (!enqueue) { + while ((cam_id = rtw_camid_search(padapter, sta->hwaddr, -1)) >= 0) { + DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(sta->hwaddr), cam_id); + clear_cam_entry(padapter, cam_id); + rtw_camid_free(padapter, cam_id); + } + } else{ + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm)); + if (psetstakey_para == NULL) { + kfree((u8 *) ph2c); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp)); + if (psetstakey_rsp == NULL) { + kfree((u8 *) ph2c); + kfree((u8 *) psetstakey_para); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_); + ph2c->rsp = (u8 *) psetstakey_rsp; + ph2c->rspsz = sizeof(struct set_stakey_rsp); + + memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN); + + psetstakey_para->algorithm = _NO_PRIVACY_; + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + } + +exit: + return res; +} + +u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr) +{ + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct cmd_obj *ph2c; + struct addBaReq_parm *paddbareq_parm; + + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm)); + if (paddbareq_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + paddbareq_parm->tid = tid; + memcpy(paddbareq_parm->addr, addr, ETH_ALEN); + + init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq)); + + /* DBG_871X("rtw_addbareq_cmd, tid =%d\n", tid); */ + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} +/* add for CONFIG_IEEE80211W, none 11w can use it */ +u8 rtw_reset_securitypriv_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = RESET_SECURITYPRIV; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 rtw_free_assoc_resources_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = FREE_ASSOC_RESOURCES; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + /* only primary padapter does this cmd */ + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue, u8 swconfig) +{ + struct cmd_obj *pcmdobj; + struct SetChannelPlan_param *setChannelPlan_param; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + u8 res = _SUCCESS; + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n")); + + /* check if allow software config */ + if (swconfig && rtw_hal_is_disable_sw_channel_plan(padapter) == true) { + res = _FAIL; + goto exit; + } + + /* check input parameter */ + if (!rtw_is_channel_plan_valid(chplan)) { + res = _FAIL; + goto exit; + } + + /* prepare cmd parameter */ + setChannelPlan_param = (struct SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param)); + if (setChannelPlan_param == NULL) { + res = _FAIL; + goto exit; + } + setChannelPlan_param->channel_plan = chplan; + + if (enqueue) { + /* need enqueue, prepare cmd_obj and enqueue */ + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + kfree((u8 *)setChannelPlan_param); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan)); + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + } else{ + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param)) + res = _FAIL; + + kfree((u8 *)setChannelPlan_param); + } + + /* do something based on res... */ + if (res == _SUCCESS) + padapter->mlmepriv.ChannelPlan = chplan; + +exit: + return res; +} + +static void collect_traffic_statistics(struct adapter *padapter) +{ + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + + /* Tx */ + pdvobjpriv->traffic_stat.tx_bytes = padapter->xmitpriv.tx_bytes; + pdvobjpriv->traffic_stat.tx_pkts = padapter->xmitpriv.tx_pkts; + pdvobjpriv->traffic_stat.tx_drop = padapter->xmitpriv.tx_drop; + + /* Rx */ + pdvobjpriv->traffic_stat.rx_bytes = padapter->recvpriv.rx_bytes; + pdvobjpriv->traffic_stat.rx_pkts = padapter->recvpriv.rx_pkts; + pdvobjpriv->traffic_stat.rx_drop = padapter->recvpriv.rx_drop; + + /* Calculate throughput in last interval */ + pdvobjpriv->traffic_stat.cur_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes - pdvobjpriv->traffic_stat.last_tx_bytes; + pdvobjpriv->traffic_stat.cur_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes - pdvobjpriv->traffic_stat.last_rx_bytes; + pdvobjpriv->traffic_stat.last_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes; + pdvobjpriv->traffic_stat.last_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes; + + pdvobjpriv->traffic_stat.cur_tx_tp = (u32)(pdvobjpriv->traffic_stat.cur_tx_bytes * 8/2/1024/1024); + pdvobjpriv->traffic_stat.cur_rx_tp = (u32)(pdvobjpriv->traffic_stat.cur_rx_bytes * 8/2/1024/1024); +} + +u8 traffic_status_watchdog(struct adapter *padapter, u8 from_timer) +{ + u8 bEnterPS = false; + u16 BusyThresholdHigh = 25; + u16 BusyThresholdLow = 10; + u16 BusyThreshold = BusyThresholdHigh; + u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false; + u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false; + + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + collect_traffic_statistics(padapter); + + /* */ + /* Determine if our traffic is busy now */ + /* */ + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) + /*&& !MgntInitAdapterInProgress(pMgntInfo)*/) { + /* if we raise bBusyTraffic in last watchdog, using lower threshold. */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic) + BusyThreshold = BusyThresholdLow; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) { + bBusyTraffic = true; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) + bRxBusyTraffic = true; + else + bTxBusyTraffic = true; + } + + /* Higher Tx/Rx data. */ + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) { + bHigherBusyTraffic = true; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) + bHigherBusyRxTraffic = true; + else + bHigherBusyTxTraffic = true; + } + + /* check traffic for powersaving. */ + if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) || + (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)) { + /* DBG_871X("(-)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */ + bEnterPS = false; + + if (bBusyTraffic == true) { + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4) + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4; + + pmlmepriv->LinkDetectInfo.TrafficTransitionCount++; + + /* DBG_871X("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount); */ + + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/) { + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30; + } + } + } else{ + /* DBG_871X("(+)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */ + + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount >= 2) + pmlmepriv->LinkDetectInfo.TrafficTransitionCount -= 2; + else + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0) + bEnterPS = true; + } + + /* LeisurePS only work in infra mode. */ + if (bEnterPS) { + if (!from_timer) + LPS_Enter(padapter, "TRAFFIC_IDLE"); + } else { + if (!from_timer) + LPS_Leave(padapter, "TRAFFIC_BUSY"); + else + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 1); + } + } else{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + int n_assoc_iface = 0; + + if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE)) + n_assoc_iface++; + + if (!from_timer && n_assoc_iface == 0) + LPS_Leave(padapter, "NON_LINKED"); + } + + 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; + + return bEnterPS; + +} + +static void dynamic_chk_wk_hdl(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv; + pmlmepriv = &(padapter->mlmepriv); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + expire_timeout_chk(padapter); + } + + /* for debug purpose */ + _linked_info_dump(padapter); + + + /* if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY) ==false) */ + { + linked_status_chk(padapter); + traffic_status_watchdog(padapter, 0); + } + + rtw_hal_dm_watchdog(padapter); + + /* check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); */ + + /* */ + /* BT-Coexist */ + /* */ + rtw_btcoex_Handler(padapter); + + + /* always call rtw_ps_processor() at last one. */ + if (is_primary_adapter(padapter)) + rtw_ps_processor(padapter); +} + +void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type); +void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + 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: + /* DBG_871X("LPS_CTRL_SCAN\n"); */ + rtw_btcoex_ScanNotify(padapter, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + /* connect */ + LPS_Leave(padapter, "LPS_CTRL_SCAN"); + } + break; + case LPS_CTRL_JOINBSS: + /* DBG_871X("LPS_CTRL_JOINBSS\n"); */ + LPS_Leave(padapter, "LPS_CTRL_JOINBSS"); + break; + case LPS_CTRL_CONNECT: + /* DBG_871X("LPS_CTRL_CONNECT\n"); */ + mstatus = 1;/* connect */ + /* Reset LPS Setting */ + pwrpriv->LpsIdleCount = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus)); + rtw_btcoex_MediaStatusNotify(padapter, mstatus); + break; + case LPS_CTRL_DISCONNECT: + /* DBG_871X("LPS_CTRL_DISCONNECT\n"); */ + mstatus = 0;/* disconnect */ + rtw_btcoex_MediaStatusNotify(padapter, mstatus); + LPS_Leave(padapter, "LPS_CTRL_DISCONNECT"); + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus)); + break; + case LPS_CTRL_SPECIAL_PACKET: + /* DBG_871X("LPS_CTRL_SPECIAL_PACKET\n"); */ + pwrpriv->DelayLPSLastTimeStamp = jiffies; + rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP); + LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET"); + break; + case LPS_CTRL_LEAVE: + /* DBG_871X("LPS_CTRL_LEAVE\n"); */ + LPS_Leave(padapter, "LPS_CTRL_LEAVE"); + break; + case LPS_CTRL_TRAFFIC_BUSY: + LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY"); + default: + break; + } +} + +u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + /* struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); */ + u8 res = _SUCCESS; + + /* if (!pwrctrlpriv->bLeisurePs) */ + /* return res; */ + + if (enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID; + pdrvextra_cmd_parm->type = lps_ctrl_type; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else{ + lps_ctrl_wk_hdl(padapter, lps_ctrl_type); + } + +exit: + return res; +} + +static void rtw_dm_in_lps_hdl(struct adapter *padapter) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS, NULL); +} + +u8 rtw_dm_in_lps_wk_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DM_IN_LPS_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +static void rtw_lps_change_dtim_hdl(struct adapter *padapter, u8 dtim) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + if (dtim <= 0 || dtim > 16) + return; + + if (rtw_btcoex_IsBtControlLps(padapter) == true) + return; + + down(&pwrpriv->lock); + + if (pwrpriv->dtim != dtim) { + DBG_871X("change DTIM from %d to %d, bFwCurrentInPSMode =%d, ps_mode =%d\n", pwrpriv->dtim, dtim, + pwrpriv->bFwCurrentInPSMode, pwrpriv->pwr_mode); + + pwrpriv->dtim = dtim; + } + + if ((pwrpriv->bFwCurrentInPSMode == true) && (pwrpriv->pwr_mode > PS_MODE_ACTIVE)) { + u8 ps_mode = pwrpriv->pwr_mode; + + /* DBG_871X("change DTIM from %d to %d, ps_mode =%d\n", pwrpriv->dtim, dtim, ps_mode); */ + + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + } + + up(&pwrpriv->lock); +} + +static void rtw_dm_ra_mask_hdl(struct adapter *padapter, struct sta_info *psta) +{ + if (psta) { + set_sta_rate(padapter, psta); + } +} + +u8 rtw_dm_ra_mask_wk_cmd(struct adapter *padapter, u8 *psta) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DM_RA_MSK_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = psta; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +static void power_saving_wk_hdl(struct adapter *padapter) +{ + rtw_ps_processor(padapter); +} + +/* add for CONFIG_IEEE80211W, none 11w can use it */ +static void reset_securitypriv_hdl(struct adapter *padapter) +{ + rtw_reset_securitypriv(padapter); +} + +static void free_assoc_resources_hdl(struct adapter *padapter) +{ + rtw_free_assoc_resources(padapter, 1); +} + +u8 rtw_ps_cmd(struct adapter *padapter) +{ + struct cmd_obj *ppscmd; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ppscmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ppscmd == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ppscmd); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ppscmd); + +exit: + return res; +} + +u32 g_wait_hiq_empty = 0; + +static void rtw_chk_hi_queue_hdl(struct adapter *padapter) +{ + struct sta_info *psta_bmc; + struct sta_priv *pstapriv = &padapter->stapriv; + unsigned long start = jiffies; + u8 empty = false; + + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + return; + + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + + while (false == empty && jiffies_to_msecs(jiffies - start) < g_wait_hiq_empty) { + msleep(100); + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + } + + if (psta_bmc->sleepq_len == 0) { + if (empty == _SUCCESS) { + bool update_tim = false; + + if (pstapriv->tim_bitmap & BIT(0)) + update_tim = true; + + pstapriv->tim_bitmap &= ~BIT(0); + pstapriv->sta_dz_bitmap &= ~BIT(0); + + if (update_tim == true) + update_beacon(padapter, _TIM_IE_, NULL, true); + } else{/* re check again */ + rtw_chk_hi_queue_cmd(padapter); + } + + } + +} + +u8 rtw_chk_hi_queue_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +struct btinfo { + u8 cid; + u8 len; + + u8 bConnection:1; + u8 bSCOeSCO:1; + u8 bInQPage:1; + u8 bACLBusy:1; + u8 bSCOBusy:1; + u8 bHID:1; + u8 bA2DP:1; + u8 bFTP:1; + + u8 retry_cnt:4; + u8 rsvd_34:1; + u8 rsvd_35:1; + u8 rsvd_36:1; + u8 rsvd_37:1; + + u8 rssi; + + u8 rsvd_50:1; + u8 rsvd_51:1; + u8 rsvd_52:1; + u8 rsvd_53:1; + u8 rsvd_54:1; + u8 rsvd_55:1; + u8 eSCO_SCO:1; + u8 Master_Slave:1; + + u8 rsvd_6; + u8 rsvd_7; +}; + +static void rtw_btinfo_hdl(struct adapter *adapter, u8 *buf, u16 buf_len) +{ + #define BTINFO_WIFI_FETCH 0x23 + #define BTINFO_BT_AUTO_RPT 0x27 + struct btinfo *info = (struct btinfo *)buf; + u8 cmd_idx; + u8 len; + + cmd_idx = info->cid; + + if (info->len > buf_len-2) { + rtw_warn_on(1); + len = buf_len-2; + } else { + len = info->len; + } + +/* define DBG_PROC_SET_BTINFO_EVT */ +#ifdef DBG_PROC_SET_BTINFO_EVT + btinfo_evt_dump(RTW_DBGDUMP, info); +#endif + + /* transform BT-FW btinfo to WiFI-FW C2H format and notify */ + if (cmd_idx == BTINFO_WIFI_FETCH) + buf[1] = 0; + else if (cmd_idx == BTINFO_BT_AUTO_RPT) + buf[1] = 2; + rtw_btcoex_BtInfoNotify(adapter, len+1, &buf[1]); +} + +u8 rtw_c2h_packet_wk_cmd(struct adapter *padapter, u8 *pbuf, u16 length) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((u8 *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = C2H_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = length; + pdrvextra_cmd_parm->pbuf = pbuf; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +/* dont call R/W in this function, beucase SDIO interrupt have claim host */ +/* or deadlock will happen and cause special-systemserver-died in android */ +u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + kfree((u8 *)ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = C2H_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = c2h_evt?16:0; + pdrvextra_cmd_parm->pbuf = c2h_evt; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; +} + +static void c2h_wk_callback(_workitem *work) +{ + struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk); + struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv); + u8 *c2h_evt; + c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter); + + evtpriv->c2h_wk_alive = true; + + while (!rtw_cbuf_empty(evtpriv->c2h_queue)) { + c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue); + if (c2h_evt != NULL) { + /* This C2H event is read, clear it */ + c2h_evt_clear(adapter); + } else{ + c2h_evt = (u8 *)rtw_malloc(16); + if (c2h_evt != NULL) { + /* This C2H event is not read, read & clear now */ + if (rtw_hal_c2h_evt_read(adapter, c2h_evt) != _SUCCESS) { + kfree(c2h_evt); + continue; + } + } + } + + /* Special pointer to trigger c2h_evt_clear only */ + if ((void *)c2h_evt == (void *)evtpriv) + continue; + + if (!rtw_hal_c2h_valid(adapter, c2h_evt)) { + kfree(c2h_evt); + continue; + } + + if (ccx_id_filter(c2h_evt) == true) { + /* Handle CCX report here */ + rtw_hal_c2h_handler(adapter, c2h_evt); + kfree(c2h_evt); + } else{ + /* Enqueue into cmd_thread for others */ + rtw_c2h_wk_cmd(adapter, c2h_evt); + } + } + + evtpriv->c2h_wk_alive = false; +} + +u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct drvextra_cmd_parm *pdrvextra_cmd; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf; + + switch (pdrvextra_cmd->ec_id) { + case DYNAMIC_CHK_WK_CID:/* only primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces */ + dynamic_chk_wk_hdl(padapter); + break; + case POWER_SAVING_CTRL_WK_CID: + power_saving_wk_hdl(padapter); + break; + case LPS_CTRL_WK_CID: + lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type); + break; + case DM_IN_LPS_WK_CID: + rtw_dm_in_lps_hdl(padapter); + break; + case LPS_CHANGE_DTIM_CID: + rtw_lps_change_dtim_hdl(padapter, (u8)pdrvextra_cmd->type); + break; + case CHECK_HIQ_WK_CID: + rtw_chk_hi_queue_hdl(padapter); + break; +#ifdef CONFIG_INTEL_WIDI + case INTEl_WIDI_WK_CID: + intel_widi_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif /* CONFIG_INTEL_WIDI */ + /* add for CONFIG_IEEE80211W, none 11w can use it */ + case RESET_SECURITYPRIV: + reset_securitypriv_hdl(padapter); + break; + case FREE_ASSOC_RESOURCES: + free_assoc_resources_hdl(padapter); + break; + case C2H_WK_CID: + rtw_hal_set_hwreg_with_buf(padapter, HW_VAR_C2H_HANDLE, pdrvextra_cmd->pbuf, pdrvextra_cmd->size); + break; + case DM_RA_MSK_WK_CID: + rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf); + break; + case BTINFO_WK_CID: + rtw_btinfo_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size); + break; + default: + break; + } + + if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size > 0) { + kfree(pdrvextra_cmd->pbuf); + } + + return H2C_SUCCESS; +} + +void rtw_survey_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (pcmd->res == H2C_DROPPED) { + /* TODO: cancel timer and do timeout handler directly... */ + /* need to make timeout handlerOS independent */ + _set_timer(&pmlmepriv->scan_to_timer, 1); + } else if (pcmd->res != H2C_SUCCESS) { + _set_timer(&pmlmepriv->scan_to_timer, 1); + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n.")); + } + + /* free cmd */ + rtw_free_cmd_obj(pcmd); +} + +void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (pcmd->res != H2C_SUCCESS) { + spin_lock_bh(&pmlmepriv->lock); + set_fwstate(pmlmepriv, _FW_LINKED); + spin_unlock_bh(&pmlmepriv->lock); + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n.")); + return; + } + /* free cmd */ + rtw_free_cmd_obj(pcmd); +} + +void rtw_joinbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (pcmd->res == H2C_DROPPED) { + /* TODO: cancel timer and do timeout handler directly... */ + /* need to make timeout handlerOS independent */ + _set_timer(&pmlmepriv->assoc_timer, 1); + } else if (pcmd->res != H2C_SUCCESS) { + _set_timer(&pmlmepriv->assoc_timer, 1); + } + + rtw_free_cmd_obj(pcmd); +} + +void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + u8 timer_cancelled; + struct sta_info *psta = NULL; + struct wlan_network *pwlan = NULL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf; + struct wlan_network *tgt_network = &(pmlmepriv->cur_network); + + if (pcmd->parmbuf == NULL) + goto exit; + + if ((pcmd->res != H2C_SUCCESS)) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback Fail ************\n\n.")); + _set_timer(&pmlmepriv->assoc_timer, 1); + } + + _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled); + + spin_lock_bh(&pmlmepriv->lock); + + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress); + if (!psta) { + psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress); + if (psta == NULL) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n")); + goto createbss_cmd_fail; + } + } + + rtw_indicate_connect(padapter); + } else{ + pwlan = _rtw_alloc_network(pmlmepriv); + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + if (pwlan == NULL) { + pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue); + if (pwlan == NULL) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error: can't get pwlan in rtw_joinbss_event_callback\n")); + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + goto createbss_cmd_fail; + } + pwlan->last_scanned = jiffies; + } else{ + list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue); + } + + pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork); + memcpy(&(pwlan->network), pnetwork, pnetwork->Length); + /* 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 */ + /* tgt_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pnetwork->Configuration.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_callback) */ + + } + +createbss_cmd_fail: + + spin_unlock_bh(&pmlmepriv->lock); +exit: + rtw_free_cmd_obj(pcmd); +} + + + +void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + + struct sta_priv *pstapriv = &padapter->stapriv; + struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *) (pcmd->rsp); + struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr); + + if (psta == NULL) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n")); + goto exit; + } +exit: + rtw_free_cmd_obj(pcmd); +} + +void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter, struct cmd_obj *pcmd) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf); + struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *) (pcmd->rsp); + struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr); + + if (psta == NULL) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n")); + goto exit; + } + + psta->aid = psta->mac_id = passocsta_rsp->cam_id; + + spin_lock_bh(&pmlmepriv->lock); + + if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)) + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + + set_fwstate(pmlmepriv, _FW_LINKED); + spin_unlock_bh(&pmlmepriv->lock); + +exit: + rtw_free_cmd_obj(pcmd); +} diff --git a/drivers/staging/rtl8723bs/core/rtw_debug.c b/drivers/staging/rtl8723bs/core/rtw_debug.c new file mode 100644 index 0000000..3db02e9 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_debug.c @@ -0,0 +1,1447 @@ +/****************************************************************************** + * + * 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_DEBUG_C_ + +#include <drv_types.h> +#include <rtw_debug.h> + +u32 GlobalDebugLevel = _drv_err_; + +#ifdef DEBUG_RTL871X + + u64 GlobalDebugComponents = \ + _module_rtl871x_xmit_c_ | + _module_xmit_osdep_c_ | + _module_rtl871x_recv_c_ | + _module_recv_osdep_c_ | + _module_rtl871x_mlme_c_ | + _module_mlme_osdep_c_ | + _module_rtl871x_sta_mgt_c_ | + _module_rtl871x_cmd_c_ | + _module_cmd_osdep_c_ | + _module_rtl871x_io_c_ | + _module_io_osdep_c_ | + _module_os_intfs_c_| + _module_rtl871x_security_c_| + _module_rtl871x_eeprom_c_| + _module_hal_init_c_| + _module_hci_hal_init_c_| + _module_rtl871x_ioctl_c_| + _module_rtl871x_ioctl_set_c_| + _module_rtl871x_ioctl_query_c_| + _module_rtl871x_pwrctrl_c_| + _module_hci_intfs_c_| + _module_hci_ops_c_| + _module_hci_ops_os_c_| + _module_rtl871x_ioctl_os_c| + _module_rtl8712_cmd_c_| + _module_hal_xmit_c_| + _module_rtl8712_recv_c_ | + _module_mp_ | + _module_efuse_; + +#endif /* DEBUG_RTL871X */ + +#include <rtw_version.h> + +void dump_drv_version(void *sel) +{ + DBG_871X_SEL_NL(sel, "%s %s\n", "rtl8723bs", DRIVERVERSION); +} + +void dump_log_level(void *sel) +{ + DBG_871X_SEL_NL(sel, "log_level:%d\n", GlobalDebugLevel); +} + +void sd_f0_reg_dump(void *sel, struct adapter *adapter) +{ + int i; + + for (i = 0x0; i <= 0xff; i++) { + if (i%16 == 0) + DBG_871X_SEL_NL(sel, "0x%02x ", i); + + DBG_871X_SEL(sel, "%02x ", rtw_sd_f0_read8(adapter, i)); + + if (i%16 == 15) + DBG_871X_SEL(sel, "\n"); + else if (i%8 == 7) + DBG_871X_SEL(sel, "\t"); + } +} + +void mac_reg_dump(void *sel, struct adapter *adapter) +{ + int i, j = 1; + + DBG_871X_SEL_NL(sel, "======= MAC REG =======\n"); + + for (i = 0x0; i < 0x800; i += 4) { + if (j%4 == 1) + DBG_871X_SEL_NL(sel, "0x%03x", i); + DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++)%4 == 0) + DBG_871X_SEL(sel, "\n"); + } +} + +void bb_reg_dump(void *sel, struct adapter *adapter) +{ + int i, j = 1; + + DBG_871X_SEL_NL(sel, "======= BB REG =======\n"); + for (i = 0x800; i < 0x1000 ; i += 4) { + if (j%4 == 1) + DBG_871X_SEL_NL(sel, "0x%03x", i); + DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++)%4 == 0) + DBG_871X_SEL(sel, "\n"); + } +} + +void rf_reg_dump(void *sel, struct adapter *adapter) +{ + int i, j = 1, path; + u32 value; + u8 rf_type = 0; + u8 path_nums = 0; + + rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + if ((RF_1T2R == rf_type) || (RF_1T1R == rf_type)) + path_nums = 1; + else + path_nums = 2; + + DBG_871X_SEL_NL(sel, "======= RF REG =======\n"); + + for (path = 0; path < path_nums; path++) { + DBG_871X_SEL_NL(sel, "RF_Path(%x)\n", path); + for (i = 0; i < 0x100; i++) { + value = rtw_hal_read_rfreg(adapter, path, i, 0xffffffff); + if (j%4 == 1) + DBG_871X_SEL_NL(sel, "0x%02x ", i); + DBG_871X_SEL(sel, " 0x%08x ", value); + if ((j++)%4 == 0) + DBG_871X_SEL(sel, "\n"); + } + } +} + +#ifdef PROC_DEBUG +ssize_t proc_set_write_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 addr, val, len; + + if (count < 3) { + DBG_871X("argument size is less than 3\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%x %x %x", &addr, &val, &len); + + if (num != 3) { + DBG_871X("invalid write_reg parameter!\n"); + return count; + } + + switch (len) { + case 1: + rtw_write8(padapter, addr, (u8)val); + break; + case 2: + rtw_write16(padapter, addr, (u16)val); + break; + case 4: + rtw_write32(padapter, addr, val); + break; + default: + DBG_871X("error write length =%d", len); + break; + } + + } + + return count; + +} + +static u32 proc_get_read_addr = 0xeeeeeeee; +static u32 proc_get_read_len = 0x4; + +int proc_get_read_reg(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + + if (proc_get_read_addr == 0xeeeeeeee) { + DBG_871X_SEL_NL(m, "address not initialized\n"); + return 0; + } + + switch (proc_get_read_len) { + case 1: + DBG_871X_SEL_NL(m, "rtw_read8(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr)); + break; + case 2: + DBG_871X_SEL_NL(m, "rtw_read16(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr)); + break; + case 4: + DBG_871X_SEL_NL(m, "rtw_read32(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr)); + break; + default: + DBG_871X_SEL_NL(m, "error read length =%d\n", proc_get_read_len); + break; + } + + return 0; +} + +ssize_t proc_set_read_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[16]; + u32 addr, len; + + if (count < 2) { + DBG_871X("argument size is less than 2\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%x %x", &addr, &len); + + if (num != 2) { + DBG_871X("invalid read_reg parameter!\n"); + return count; + } + + proc_get_read_addr = addr; + + proc_get_read_len = len; + } + + return count; + +} + +int proc_get_fwstate(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + DBG_871X_SEL_NL(m, "fwstate = 0x%x\n", get_fwstate(pmlmepriv)); + + return 0; +} + +int proc_get_sec_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct security_priv *sec = &padapter->securitypriv; + + DBG_871X_SEL_NL(m, "auth_alg = 0x%x, enc_alg = 0x%x, auth_type = 0x%x, enc_type = 0x%x\n", + sec->dot11AuthAlgrthm, sec->dot11PrivacyAlgrthm, + sec->ndisauthtype, sec->ndisencryptstatus); + + DBG_871X_SEL_NL(m, "hw_decrypted =%d\n", sec->hw_decrypted); + +#ifdef DBG_SW_SEC_CNT + DBG_871X_SEL_NL(m, "wep_sw_enc_cnt =%llu, %llu, %llu\n" + , sec->wep_sw_enc_cnt_bc, sec->wep_sw_enc_cnt_mc, sec->wep_sw_enc_cnt_uc); + DBG_871X_SEL_NL(m, "wep_sw_dec_cnt =%llu, %llu, %llu\n" + , sec->wep_sw_dec_cnt_bc, sec->wep_sw_dec_cnt_mc, sec->wep_sw_dec_cnt_uc); + + DBG_871X_SEL_NL(m, "tkip_sw_enc_cnt =%llu, %llu, %llu\n" + , sec->tkip_sw_enc_cnt_bc, sec->tkip_sw_enc_cnt_mc, sec->tkip_sw_enc_cnt_uc); + DBG_871X_SEL_NL(m, "tkip_sw_dec_cnt =%llu, %llu, %llu\n" + , sec->tkip_sw_dec_cnt_bc, sec->tkip_sw_dec_cnt_mc, sec->tkip_sw_dec_cnt_uc); + + DBG_871X_SEL_NL(m, "aes_sw_enc_cnt =%llu, %llu, %llu\n" + , sec->aes_sw_enc_cnt_bc, sec->aes_sw_enc_cnt_mc, sec->aes_sw_enc_cnt_uc); + DBG_871X_SEL_NL(m, "aes_sw_dec_cnt =%llu, %llu, %llu\n" + , sec->aes_sw_dec_cnt_bc, sec->aes_sw_dec_cnt_mc, sec->aes_sw_dec_cnt_uc); +#endif /* DBG_SW_SEC_CNT */ + + return 0; +} + +int proc_get_mlmext_state(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + DBG_871X_SEL_NL(m, "pmlmeinfo->state = 0x%x\n", pmlmeinfo->state); + + return 0; +} + +int proc_get_roam_flags(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + + DBG_871X_SEL_NL(m, "0x%02x\n", rtw_roam_flags(adapter)); + + return 0; +} + +ssize_t proc_set_roam_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + + char tmp[32]; + u8 flags; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%hhx", &flags); + + if (num == 1) + rtw_assign_roam_flags(adapter, flags); + } + + return count; + +} + +int proc_get_roam_param(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + DBG_871X_SEL_NL(m, "%12s %12s %11s\n", "rssi_diff_th", "scanr_exp_ms", "scan_int_ms"); + DBG_871X_SEL_NL(m, "%-12u %-12u %-11u\n" + , mlme->roam_rssi_diff_th + , mlme->roam_scanr_exp_ms + , mlme->roam_scan_int_ms + ); + + return 0; +} + +ssize_t proc_set_roam_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + char tmp[32]; + u8 rssi_diff_th; + u32 scanr_exp_ms; + u32 scan_int_ms; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%hhu %u %u", &rssi_diff_th, &scanr_exp_ms, &scan_int_ms); + + if (num >= 1) + mlme->roam_rssi_diff_th = rssi_diff_th; + if (num >= 2) + mlme->roam_scanr_exp_ms = scanr_exp_ms; + if (num >= 3) + mlme->roam_scan_int_ms = scan_int_ms; + } + + return count; + +} + +ssize_t proc_set_roam_tgt_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + + char tmp[32]; + u8 addr[ETH_ALEN]; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", addr, addr+1, addr+2, addr+3, addr+4, addr+5); + if (num == 6) + memcpy(adapter->mlmepriv.roam_tgt_addr, addr, ETH_ALEN); + + DBG_871X("set roam_tgt_addr to "MAC_FMT"\n", MAC_ARG(adapter->mlmepriv.roam_tgt_addr)); + } + + return count; +} + +int proc_get_qos_option(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + DBG_871X_SEL_NL(m, "qos_option =%d\n", pmlmepriv->qospriv.qos_option); + + return 0; +} + +int proc_get_ht_option(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + DBG_871X_SEL_NL(m, "ht_option =%d\n", pmlmepriv->htpriv.ht_option); + + return 0; +} + +int proc_get_rf_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + DBG_871X_SEL_NL(m, "cur_ch =%d, cur_bw =%d, cur_ch_offet =%d\n", + pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset); + + DBG_871X_SEL_NL(m, "oper_ch =%d, oper_bw =%d, oper_ch_offet =%d\n", + rtw_get_oper_ch(padapter), rtw_get_oper_bw(padapter), rtw_get_oper_choffset(padapter)); + + return 0; +} + +int proc_get_survey_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct __queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct list_head *plist, *phead; + s32 notify_signal; + s16 notify_noise = 0; + u16 index = 0; + + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + phead = get_list_head(queue); + plist = phead ? get_next(phead) : NULL; + if ((!phead) || (!plist)) { + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + return 0; + } + + DBG_871X_SEL_NL(m, "%5s %-17s %3s %-3s %-4s %-4s %5s %s\n", "index", "bssid", "ch", "RSSI", "SdBm", "Noise", "age", "ssid"); + while (1) { + if (phead == plist) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (!pnetwork) + break; + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true && + is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) { + notify_signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);/*dbm*/ + } else { + notify_signal = translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength);/*dbm*/ + } + + #if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR) + rtw_hal_get_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &(pnetwork->network.Configuration.DSConfig), &(notify_noise)); + #endif + + DBG_871X_SEL_NL(m, "%5d "MAC_FMT" %3d %3d %4d %4d %5d %s\n", + ++index, + MAC_ARG(pnetwork->network.MacAddress), + pnetwork->network.Configuration.DSConfig, + (int)pnetwork->network.Rssi, + notify_signal, + notify_noise, + jiffies_to_msecs(jiffies - pnetwork->last_scanned), + /*translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength),*/ + pnetwork->network.Ssid.Ssid); + plist = get_next(plist); + } + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + + return 0; +} + +int proc_get_ap_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct sta_info *psta; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct sta_priv *pstapriv = &padapter->stapriv; + + psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress); + if (psta) { + int i; + struct recv_reorder_ctrl *preorder_ctrl; + + DBG_871X_SEL_NL(m, "SSID =%s\n", cur_network->network.Ssid.Ssid); + DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr)); + DBG_871X_SEL_NL(m, "cur_channel =%d, cur_bwmode =%d, cur_ch_offset =%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset); + DBG_871X_SEL_NL(m, "wireless_mode = 0x%x, rtsen =%d, cts2slef =%d\n", psta->wireless_mode, psta->rtsen, psta->cts2self); + DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n", psta->state, psta->aid, psta->mac_id, psta->raid); + DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate); + DBG_871X_SEL_NL(m, "bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m); + DBG_871X_SEL_NL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable); + DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap); + DBG_871X_SEL_NL(m, "ldpc_cap = 0x%x, stbc_cap = 0x%x, beamform_cap = 0x%x\n", psta->htpriv.ldpc_cap, psta->htpriv.stbc_cap, psta->htpriv.beamform_cap); + + for (i = 0; i < 16; i++) { + preorder_ctrl = &psta->recvreorder_ctrl[i]; + if (preorder_ctrl->enable) { + DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n", i, preorder_ctrl->indicate_seq); + } + } + + } else{ + DBG_871X_SEL_NL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress)); + } + + return 0; +} + +int proc_get_adapter_state(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + + DBG_871X_SEL_NL(m, "name =%s, bSurpriseRemoved =%d, bDriverStopped =%d\n", + dev->name, padapter->bSurpriseRemoved, padapter->bDriverStopped); + + return 0; +} + +int proc_get_trx_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + int i; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct hw_xmit *phwxmit; + + DBG_871X_SEL_NL(m, "free_xmitbuf_cnt =%d, free_xmitframe_cnt =%d\n" + , pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt); + DBG_871X_SEL_NL(m, "free_ext_xmitbuf_cnt =%d, free_xframe_ext_cnt =%d\n" + , pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt); + DBG_871X_SEL_NL(m, "free_recvframe_cnt =%d\n" + , precvpriv->free_recvframe_cnt); + + for (i = 0; i < 4; i++) { + phwxmit = pxmitpriv->hwxmits + i; + DBG_871X_SEL_NL(m, "%d, hwq.accnt =%d\n", i, phwxmit->accnt); + } + + return 0; +} + +int proc_get_rate_ctl(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + + if (adapter->fix_rate != 0xff) { + DBG_871X_SEL_NL(m, "FIX\n"); + DBG_871X_SEL_NL(m, "0x%02x\n", adapter->fix_rate); + } else { + DBG_871X_SEL_NL(m, "RA\n"); + } + + return 0; +} + +ssize_t proc_set_rate_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 fix_rate; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%hhx", &fix_rate); + + if (num >= 1) + adapter->fix_rate = fix_rate; + } + + return count; +} + +ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%hhu %hhu", &g_fwdl_chksum_fail, &g_fwdl_wintint_rdy_fail); + } + + return count; +} + +ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%u", &g_wait_hiq_empty); + } + + return count; +} + +int proc_get_suspend_resume_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = padapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; + + DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_cnt); + DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_cnt); + DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_error_cnt); + DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_error_cnt); + DBG_871X_SEL_NL(m, "dbg_sdio_init_error_cnt =%d\n", pdbgpriv->dbg_sdio_init_error_cnt); + DBG_871X_SEL_NL(m, "dbg_sdio_deinit_error_cnt =%d\n", pdbgpriv->dbg_sdio_deinit_error_cnt); + DBG_871X_SEL_NL(m, "dbg_suspend_error_cnt =%d\n", pdbgpriv->dbg_suspend_error_cnt); + DBG_871X_SEL_NL(m, "dbg_suspend_cnt =%d\n", pdbgpriv->dbg_suspend_cnt); + DBG_871X_SEL_NL(m, "dbg_resume_cnt =%d\n", pdbgpriv->dbg_resume_cnt); + DBG_871X_SEL_NL(m, "dbg_resume_error_cnt =%d\n", pdbgpriv->dbg_resume_error_cnt); + DBG_871X_SEL_NL(m, "dbg_deinit_fail_cnt =%d\n", pdbgpriv->dbg_deinit_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_carddisable_cnt =%d\n", pdbgpriv->dbg_carddisable_cnt); + DBG_871X_SEL_NL(m, "dbg_ps_insuspend_cnt =%d\n", pdbgpriv->dbg_ps_insuspend_cnt); + DBG_871X_SEL_NL(m, "dbg_dev_unload_inIPS_cnt =%d\n", pdbgpriv->dbg_dev_unload_inIPS_cnt); + DBG_871X_SEL_NL(m, "dbg_scan_pwr_state_cnt =%d\n", pdbgpriv->dbg_scan_pwr_state_cnt); + DBG_871X_SEL_NL(m, "dbg_downloadfw_pwr_state_cnt =%d\n", pdbgpriv->dbg_downloadfw_pwr_state_cnt); + DBG_871X_SEL_NL(m, "dbg_carddisable_error_cnt =%d\n", pdbgpriv->dbg_carddisable_error_cnt); + DBG_871X_SEL_NL(m, "dbg_fw_read_ps_state_fail_cnt =%d\n", pdbgpriv->dbg_fw_read_ps_state_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_leave_ips_fail_cnt =%d\n", pdbgpriv->dbg_leave_ips_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_leave_lps_fail_cnt =%d\n", pdbgpriv->dbg_leave_lps_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_h2c_leave32k_fail_cnt =%d\n", pdbgpriv->dbg_h2c_leave32k_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_diswow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_diswow_dload_fw_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_enwow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_enwow_dload_fw_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_ips_drvopen_fail_cnt =%d\n", pdbgpriv->dbg_ips_drvopen_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_poll_fail_cnt =%d\n", pdbgpriv->dbg_poll_fail_cnt); + DBG_871X_SEL_NL(m, "dbg_rpwm_toogle_cnt =%d\n", pdbgpriv->dbg_rpwm_toogle_cnt); + DBG_871X_SEL_NL(m, "dbg_rpwm_timeout_fail_cnt =%d\n", pdbgpriv->dbg_rpwm_timeout_fail_cnt); + + return 0; +} + +#ifdef CONFIG_DBG_COUNTER + +int proc_get_rx_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct rx_logs *rx_logs = &padapter->rx_logs; + + DBG_871X_SEL_NL(m, + "intf_rx =%d\n" + "intf_rx_err_recvframe =%d\n" + "intf_rx_err_skb =%d\n" + "intf_rx_report =%d\n" + "core_rx =%d\n" + "core_rx_pre =%d\n" + "core_rx_pre_ver_err =%d\n" + "core_rx_pre_mgmt =%d\n" + "core_rx_pre_mgmt_err_80211w =%d\n" + "core_rx_pre_mgmt_err =%d\n" + "core_rx_pre_ctrl =%d\n" + "core_rx_pre_ctrl_err =%d\n" + "core_rx_pre_data =%d\n" + "core_rx_pre_data_wapi_seq_err =%d\n" + "core_rx_pre_data_wapi_key_err =%d\n" + "core_rx_pre_data_handled =%d\n" + "core_rx_pre_data_err =%d\n" + "core_rx_pre_data_unknown =%d\n" + "core_rx_pre_unknown =%d\n" + "core_rx_enqueue =%d\n" + "core_rx_dequeue =%d\n" + "core_rx_post =%d\n" + "core_rx_post_decrypt =%d\n" + "core_rx_post_decrypt_wep =%d\n" + "core_rx_post_decrypt_tkip =%d\n" + "core_rx_post_decrypt_aes =%d\n" + "core_rx_post_decrypt_wapi =%d\n" + "core_rx_post_decrypt_hw =%d\n" + "core_rx_post_decrypt_unknown =%d\n" + "core_rx_post_decrypt_err =%d\n" + "core_rx_post_defrag_err =%d\n" + "core_rx_post_portctrl_err =%d\n" + "core_rx_post_indicate =%d\n" + "core_rx_post_indicate_in_oder =%d\n" + "core_rx_post_indicate_reoder =%d\n" + "core_rx_post_indicate_err =%d\n" + "os_indicate =%d\n" + "os_indicate_ap_mcast =%d\n" + "os_indicate_ap_forward =%d\n" + "os_indicate_ap_self =%d\n" + "os_indicate_err =%d\n" + "os_netif_ok =%d\n" + "os_netif_err =%d\n", + rx_logs->intf_rx, + rx_logs->intf_rx_err_recvframe, + rx_logs->intf_rx_err_skb, + rx_logs->intf_rx_report, + rx_logs->core_rx, + rx_logs->core_rx_pre, + rx_logs->core_rx_pre_ver_err, + rx_logs->core_rx_pre_mgmt, + rx_logs->core_rx_pre_mgmt_err_80211w, + rx_logs->core_rx_pre_mgmt_err, + rx_logs->core_rx_pre_ctrl, + rx_logs->core_rx_pre_ctrl_err, + rx_logs->core_rx_pre_data, + rx_logs->core_rx_pre_data_wapi_seq_err, + rx_logs->core_rx_pre_data_wapi_key_err, + rx_logs->core_rx_pre_data_handled, + rx_logs->core_rx_pre_data_err, + rx_logs->core_rx_pre_data_unknown, + rx_logs->core_rx_pre_unknown, + rx_logs->core_rx_enqueue, + rx_logs->core_rx_dequeue, + rx_logs->core_rx_post, + rx_logs->core_rx_post_decrypt, + rx_logs->core_rx_post_decrypt_wep, + rx_logs->core_rx_post_decrypt_tkip, + rx_logs->core_rx_post_decrypt_aes, + rx_logs->core_rx_post_decrypt_wapi, + rx_logs->core_rx_post_decrypt_hw, + rx_logs->core_rx_post_decrypt_unknown, + rx_logs->core_rx_post_decrypt_err, + rx_logs->core_rx_post_defrag_err, + rx_logs->core_rx_post_portctrl_err, + rx_logs->core_rx_post_indicate, + rx_logs->core_rx_post_indicate_in_oder, + rx_logs->core_rx_post_indicate_reoder, + rx_logs->core_rx_post_indicate_err, + rx_logs->os_indicate, + rx_logs->os_indicate_ap_mcast, + rx_logs->os_indicate_ap_forward, + rx_logs->os_indicate_ap_self, + rx_logs->os_indicate_err, + rx_logs->os_netif_ok, + rx_logs->os_netif_err + ); + + return 0; +} + +int proc_get_tx_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct tx_logs *tx_logs = &padapter->tx_logs; + + DBG_871X_SEL_NL(m, + "os_tx =%d\n" + "os_tx_err_up =%d\n" + "os_tx_err_xmit =%d\n" + "os_tx_m2u =%d\n" + "os_tx_m2u_ignore_fw_linked =%d\n" + "os_tx_m2u_ignore_self =%d\n" + "os_tx_m2u_entry =%d\n" + "os_tx_m2u_entry_err_xmit =%d\n" + "os_tx_m2u_entry_err_skb =%d\n" + "os_tx_m2u_stop =%d\n" + "core_tx =%d\n" + "core_tx_err_pxmitframe =%d\n" + "core_tx_err_brtx =%d\n" + "core_tx_upd_attrib =%d\n" + "core_tx_upd_attrib_adhoc =%d\n" + "core_tx_upd_attrib_sta =%d\n" + "core_tx_upd_attrib_ap =%d\n" + "core_tx_upd_attrib_unknown =%d\n" + "core_tx_upd_attrib_dhcp =%d\n" + "core_tx_upd_attrib_icmp =%d\n" + "core_tx_upd_attrib_active =%d\n" + "core_tx_upd_attrib_err_ucast_sta =%d\n" + "core_tx_upd_attrib_err_ucast_ap_link =%d\n" + "core_tx_upd_attrib_err_sta =%d\n" + "core_tx_upd_attrib_err_link =%d\n" + "core_tx_upd_attrib_err_sec =%d\n" + "core_tx_ap_enqueue_warn_fwstate =%d\n" + "core_tx_ap_enqueue_warn_sta =%d\n" + "core_tx_ap_enqueue_warn_nosta =%d\n" + "core_tx_ap_enqueue_warn_link =%d\n" + "core_tx_ap_enqueue_warn_trigger =%d\n" + "core_tx_ap_enqueue_mcast =%d\n" + "core_tx_ap_enqueue_ucast =%d\n" + "core_tx_ap_enqueue =%d\n" + "intf_tx =%d\n" + "intf_tx_pending_ac =%d\n" + "intf_tx_pending_fw_under_survey =%d\n" + "intf_tx_pending_fw_under_linking =%d\n" + "intf_tx_pending_xmitbuf =%d\n" + "intf_tx_enqueue =%d\n" + "core_tx_enqueue =%d\n" + "core_tx_enqueue_class =%d\n" + "core_tx_enqueue_class_err_sta =%d\n" + "core_tx_enqueue_class_err_nosta =%d\n" + "core_tx_enqueue_class_err_fwlink =%d\n" + "intf_tx_direct =%d\n" + "intf_tx_direct_err_coalesce =%d\n" + "intf_tx_dequeue =%d\n" + "intf_tx_dequeue_err_coalesce =%d\n" + "intf_tx_dump_xframe =%d\n" + "intf_tx_dump_xframe_err_txdesc =%d\n" + "intf_tx_dump_xframe_err_port =%d\n", + tx_logs->os_tx, + tx_logs->os_tx_err_up, + tx_logs->os_tx_err_xmit, + tx_logs->os_tx_m2u, + tx_logs->os_tx_m2u_ignore_fw_linked, + tx_logs->os_tx_m2u_ignore_self, + tx_logs->os_tx_m2u_entry, + tx_logs->os_tx_m2u_entry_err_xmit, + tx_logs->os_tx_m2u_entry_err_skb, + tx_logs->os_tx_m2u_stop, + tx_logs->core_tx, + tx_logs->core_tx_err_pxmitframe, + tx_logs->core_tx_err_brtx, + tx_logs->core_tx_upd_attrib, + tx_logs->core_tx_upd_attrib_adhoc, + tx_logs->core_tx_upd_attrib_sta, + tx_logs->core_tx_upd_attrib_ap, + tx_logs->core_tx_upd_attrib_unknown, + tx_logs->core_tx_upd_attrib_dhcp, + tx_logs->core_tx_upd_attrib_icmp, + tx_logs->core_tx_upd_attrib_active, + tx_logs->core_tx_upd_attrib_err_ucast_sta, + tx_logs->core_tx_upd_attrib_err_ucast_ap_link, + tx_logs->core_tx_upd_attrib_err_sta, + tx_logs->core_tx_upd_attrib_err_link, + tx_logs->core_tx_upd_attrib_err_sec, + tx_logs->core_tx_ap_enqueue_warn_fwstate, + tx_logs->core_tx_ap_enqueue_warn_sta, + tx_logs->core_tx_ap_enqueue_warn_nosta, + tx_logs->core_tx_ap_enqueue_warn_link, + tx_logs->core_tx_ap_enqueue_warn_trigger, + tx_logs->core_tx_ap_enqueue_mcast, + tx_logs->core_tx_ap_enqueue_ucast, + tx_logs->core_tx_ap_enqueue, + tx_logs->intf_tx, + tx_logs->intf_tx_pending_ac, + tx_logs->intf_tx_pending_fw_under_survey, + tx_logs->intf_tx_pending_fw_under_linking, + tx_logs->intf_tx_pending_xmitbuf, + tx_logs->intf_tx_enqueue, + tx_logs->core_tx_enqueue, + tx_logs->core_tx_enqueue_class, + tx_logs->core_tx_enqueue_class_err_sta, + tx_logs->core_tx_enqueue_class_err_nosta, + tx_logs->core_tx_enqueue_class_err_fwlink, + tx_logs->intf_tx_direct, + tx_logs->intf_tx_direct_err_coalesce, + tx_logs->intf_tx_dequeue, + tx_logs->intf_tx_dequeue_err_coalesce, + tx_logs->intf_tx_dump_xframe, + tx_logs->intf_tx_dump_xframe_err_txdesc, + tx_logs->intf_tx_dump_xframe_err_port + ); + + return 0; +} + +int proc_get_int_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + + DBG_871X_SEL_NL(m, + "all =%d\n" + "err =%d\n" + "tbdok =%d\n" + "tbder =%d\n" + "bcnderr =%d\n" + "bcndma =%d\n" + "bcndma_e =%d\n" + "rx =%d\n" + "rx_rdu =%d\n" + "rx_fovw =%d\n" + "txfovw =%d\n" + "mgntok =%d\n" + "highdok =%d\n" + "bkdok =%d\n" + "bedok =%d\n" + "vidok =%d\n" + "vodok =%d\n", + padapter->int_logs.all, + padapter->int_logs.err, + padapter->int_logs.tbdok, + padapter->int_logs.tbder, + padapter->int_logs.bcnderr, + padapter->int_logs.bcndma, + padapter->int_logs.bcndma_e, + padapter->int_logs.rx, + padapter->int_logs.rx_rdu, + padapter->int_logs.rx_fovw, + padapter->int_logs.txfovw, + padapter->int_logs.mgntok, + padapter->int_logs.highdok, + padapter->int_logs.bkdok, + padapter->int_logs.bedok, + padapter->int_logs.vidok, + padapter->int_logs.vodok + ); + + return 0; +} + +#endif /* CONFIG_DBG_COUNTER*/ + +int proc_get_rx_signal(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + + DBG_871X_SEL_NL(m, "rssi:%d\n", padapter->recvpriv.rssi); + /*DBG_871X_SEL_NL(m, "rxpwdb:%d\n", padapter->recvpriv.rxpwdb);*/ + DBG_871X_SEL_NL(m, "signal_strength:%u\n", padapter->recvpriv.signal_strength); + DBG_871X_SEL_NL(m, "signal_qual:%u\n", padapter->recvpriv.signal_qual); + DBG_871X_SEL_NL(m, "noise:%d\n", padapter->recvpriv.noise); + rtw_odm_get_perpkt_rssi(m, padapter); + #ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA + rtw_get_raw_rssi_info(m, padapter); + #endif + return 0; +} + + +int proc_get_hw_status(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = padapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; + + DBG_871X_SEL_NL(m, "RX FIFO full count: last_time =%lld, current_time =%lld, differential =%lld\n" + , pdbgpriv->dbg_rx_fifo_last_overflow, pdbgpriv->dbg_rx_fifo_curr_overflow, pdbgpriv->dbg_rx_fifo_diff_overflow); + + return 0; +} + +ssize_t proc_set_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 is_signal_dbg, signal_strength; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength); + + is_signal_dbg = is_signal_dbg == 0?0:1; + + if (is_signal_dbg && num != 2) + return count; + + signal_strength = signal_strength > 100?100:signal_strength; + + padapter->recvpriv.is_signal_dbg = is_signal_dbg; + padapter->recvpriv.signal_strength_dbg = signal_strength; + + if (is_signal_dbg) + DBG_871X("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength); + else + DBG_871X("set %s\n", "HW_SIGNAL_STRENGTH"); + + } + + return count; + +} + +int proc_get_ht_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + DBG_871X_SEL_NL(m, "%d\n", pregpriv->ht_enable); + + return 0; +} + +ssize_t proc_set_ht_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%d ", &mode); + + if (pregpriv && mode < 2) { + pregpriv->ht_enable = mode; + printk("ht_enable =%d\n", pregpriv->ht_enable); + } + } + + return count; + +} + +int proc_get_bw_mode(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + DBG_871X_SEL_NL(m, "0x%02x\n", pregpriv->bw_mode); + + return 0; +} + +ssize_t proc_set_bw_mode(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%d ", &mode); + + if (pregpriv && mode < 2) { + + pregpriv->bw_mode = mode; + printk("bw_mode =%d\n", mode); + + } + } + + return count; + +} + +int proc_get_ampdu_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + DBG_871X_SEL_NL(m, "%d\n", pregpriv->ampdu_enable); + + return 0; +} + +ssize_t proc_set_ampdu_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + sscanf(tmp, "%d ", &mode); + + if (pregpriv && mode < 3) { + pregpriv->ampdu_enable = mode; + printk("ampdu_enable =%d\n", mode); + } + + } + + return count; + +} + +int proc_get_rx_ampdu(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pregpriv) + DBG_871X_SEL_NL(m, + "bAcceptAddbaReq = %d , 0:Reject AP's Add BA req, 1:Accept AP's Add BA req.\n", pmlmeinfo->bAcceptAddbaReq + ); + + return 0; +} + +ssize_t proc_set_rx_ampdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + sscanf(tmp, "%d ", &mode); + + if (pregpriv && mode < 2) { + pmlmeinfo->bAcceptAddbaReq = mode; + DBG_871X("pmlmeinfo->bAcceptAddbaReq =%d\n", pmlmeinfo->bAcceptAddbaReq); + if (mode == 0) { + /*tear down Rx AMPDU*/ + send_delba(padapter, 0, get_my_bssid(&(pmlmeinfo->network)));/* recipient*/ + } + } + + } + + return count; +} + +int proc_get_en_fwps(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + DBG_871X_SEL_NL(m, "check_fw_ps = %d , 1:enable get FW PS state , 0: disable get FW PS state\n" + , pregpriv->check_fw_ps); + + return 0; +} + +ssize_t proc_set_en_fwps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%d ", &mode); + + if (pregpriv && mode < 2) { + pregpriv->check_fw_ps = mode; + DBG_871X("pregpriv->check_fw_ps =%d\n", pregpriv->check_fw_ps); + } + } + return count; +} + +int proc_get_rx_stbc(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + DBG_871X_SEL_NL(m, "%d\n", pregpriv->rx_stbc); + + return 0; +} + +ssize_t proc_set_rx_stbc(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + sscanf(tmp, "%d ", &mode); + + if (pregpriv && (mode == 0 || mode == 1 || + mode == 2 || mode == 3)) { + pregpriv->rx_stbc = mode; + printk("rx_stbc =%d\n", mode); + } + } + + return count; + +} + +int proc_get_rssi_disp(struct seq_file *m, void *v) +{ + return 0; +} + +ssize_t proc_set_rssi_disp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 enable = 0; + + if (count < 1) { + DBG_8192C("argument size is less than 1\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + int num = sscanf(tmp, "%x", &enable); + + if (num != 1) { + DBG_8192C("invalid set_rssi_disp parameter!\n"); + return count; + } + + if (enable) { + DBG_8192C("Linked info Function Enable\n"); + padapter->bLinkInfoDump = enable; + } else { + DBG_8192C("Linked info Function Disable\n"); + padapter->bLinkInfoDump = 0; + } + } + return count; +} + +int proc_get_all_sta_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct sta_info *psta; + struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev); + struct sta_priv *pstapriv = &padapter->stapriv; + int i, j; + struct list_head *plist, *phead; + struct recv_reorder_ctrl *preorder_ctrl; + + DBG_871X_SEL_NL(m, "sta_dz_bitmap = 0x%x, tim_bitmap = 0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap); + + spin_lock_bh(&pstapriv->sta_hash_lock); + + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while (phead != plist) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + plist = get_next(plist); + + DBG_871X_SEL_NL(m, "==============================\n"); + DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n", + MAC_ARG(psta->hwaddr)); + DBG_871X_SEL_NL(m, "rtsen =%d, cts2slef =%d\n", + psta->rtsen, psta->cts2self); + DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n", + psta->state, psta->aid, psta->mac_id, + psta->raid); + DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n", + psta->qos_option, + psta->htpriv.ht_option, + psta->init_rate); + DBG_871X_SEL_NL(m, "bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n", + psta->bw_mode, psta->htpriv.ch_offset, + psta->htpriv.sgi_20m, + psta->htpriv.sgi_40m); + DBG_871X_SEL_NL(m, "ampdu_enable = %d\n", + psta->htpriv.ampdu_enable); + DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", + psta->htpriv.agg_enable_bitmap, + psta->htpriv.candidate_tid_bitmap); + DBG_871X_SEL_NL(m, "sleepq_len =%d\n", + psta->sleepq_len); + DBG_871X_SEL_NL(m, "sta_xmitpriv.vo_q_qcnt =%d\n", + psta->sta_xmitpriv.vo_q.qcnt); + DBG_871X_SEL_NL(m, "sta_xmitpriv.vi_q_qcnt =%d\n", + psta->sta_xmitpriv.vi_q.qcnt); + DBG_871X_SEL_NL(m, "sta_xmitpriv.be_q_qcnt =%d\n", + psta->sta_xmitpriv.be_q.qcnt); + DBG_871X_SEL_NL(m, "sta_xmitpriv.bk_q_qcnt =%d\n", + psta->sta_xmitpriv.bk_q.qcnt); + + DBG_871X_SEL_NL(m, "capability = 0x%x\n", + psta->capability); + DBG_871X_SEL_NL(m, "flags = 0x%x\n", psta->flags); + DBG_871X_SEL_NL(m, "wpa_psk = 0x%x\n", psta->wpa_psk); + DBG_871X_SEL_NL(m, "wpa2_group_cipher = 0x%x\n", + psta->wpa2_group_cipher); + DBG_871X_SEL_NL(m, "wpa2_pairwise_cipher = 0x%x\n", + psta->wpa2_pairwise_cipher); + DBG_871X_SEL_NL(m, "qos_info = 0x%x\n", psta->qos_info); + DBG_871X_SEL_NL(m, "dot118021XPrivacy = 0x%x\n", + psta->dot118021XPrivacy); + + for (j = 0; j < 16; j++) { + preorder_ctrl = &psta->recvreorder_ctrl[j]; + if (preorder_ctrl->enable) + DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n", + j, preorder_ctrl->indicate_seq); + } + DBG_871X_SEL_NL(m, "==============================\n"); + } + } + + spin_unlock_bh(&pstapriv->sta_hash_lock); + + return 0; +} + +int proc_get_btcoex_dbg(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter; + char buf[512] = {0}; + padapter = (struct adapter *)rtw_netdev_priv(dev); + + rtw_btcoex_GetDBG(padapter, buf, 512); + + DBG_871X_SEL(m, "%s", buf); + + return 0; +} + +ssize_t proc_set_btcoex_dbg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct adapter *padapter; + u8 tmp[80] = {0}; + u32 module[2] = {0}; + u32 num; + + padapter = (struct adapter *)rtw_netdev_priv(dev); + +/* DBG_871X("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));*/ + + if (NULL == buffer) { + DBG_871X(FUNC_ADPT_FMT ": input buffer is NULL!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + if (count < 1) { + DBG_871X(FUNC_ADPT_FMT ": input length is 0!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + num = count; + if (num > (sizeof(tmp) - 1)) + num = (sizeof(tmp) - 1); + + if (copy_from_user(tmp, buffer, num)) { + DBG_871X(FUNC_ADPT_FMT ": copy buffer from user space FAIL!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + num = sscanf(tmp, "%x %x", module, module+1); + if (1 == num) { + if (0 == module[0]) + memset(module, 0, sizeof(module)); + else + memset(module, 0xFF, sizeof(module)); + } else if (2 != num) { + DBG_871X(FUNC_ADPT_FMT ": input(\"%s\") format incorrect!\n", + FUNC_ADPT_ARG(padapter), tmp); + + if (0 == num) + return -EFAULT; + } + + DBG_871X(FUNC_ADPT_FMT ": input 0x%08X 0x%08X\n", + FUNC_ADPT_ARG(padapter), module[0], module[1]); + rtw_btcoex_SetDBG(padapter, module); + + return count; +} + +int proc_get_btcoex_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct adapter *padapter; + const u32 bufsize = 30*100; + u8 *pbuf = NULL; + + padapter = (struct adapter *)rtw_netdev_priv(dev); + + pbuf = rtw_zmalloc(bufsize); + if (NULL == pbuf) { + return -ENOMEM; + } + + rtw_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize); + + DBG_871X_SEL(m, "%s\n", pbuf); + + kfree(pbuf); + + return 0; +} + +#endif diff --git a/drivers/staging/rtl8723bs/core/rtw_eeprom.c b/drivers/staging/rtl8723bs/core/rtw_eeprom.c new file mode 100644 index 0000000..35031a7 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_eeprom.c @@ -0,0 +1,369 @@ +/****************************************************************************** + * + * 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_EEPROM_C_ + +#include <drv_conf.h> +#include <osdep_service.h> +#include <drv_types.h> + +void up_clk(_adapter *padapter, u16 *x) +{ +_func_enter_; + *x = *x | _EESK; + rtw_write8(padapter, EE_9346CR, (u8)*x); + udelay(CLOCK_RATE); + +_func_exit_; + +} + +void down_clk(_adapter *padapter, u16 *x) +{ +_func_enter_; + *x = *x & ~_EESK; + rtw_write8(padapter, EE_9346CR, (u8)*x); + udelay(CLOCK_RATE); +_func_exit_; +} + +void shift_out_bits(_adapter *padapter, u16 data, u16 count) +{ + u16 x, mask; +_func_enter_; + + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + mask = 0x01 << (count - 1); + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDO | _EEDI); + + do { + x &= ~_EEDI; + if (data & mask) + x |= _EEDI; + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + rtw_write8(padapter, EE_9346CR, (u8)x); + udelay(CLOCK_RATE); + up_clk(padapter, &x); + down_clk(padapter, &x); + mask = mask >> 1; + } while (mask); + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + x &= ~_EEDI; + rtw_write8(padapter, EE_9346CR, (u8)x); +out: +_func_exit_; +} + +u16 shift_in_bits(_adapter *padapter) +{ + u16 x, d = 0, i; +_func_enter_; + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDO | _EEDI); + d = 0; + + for (i = 0; i < 16; i++) { + d = d << 1; + up_clk(padapter, &x); + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDI); + if (x & _EEDO) + d |= 1; + + down_clk(padapter, &x); + } +out: +_func_exit_; + + return d; +} + +void standby(_adapter *padapter) +{ + u8 x; +_func_enter_; + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EECS | _EESK); + rtw_write8(padapter, EE_9346CR, x); + + udelay(CLOCK_RATE); + x |= _EECS; + rtw_write8(padapter, EE_9346CR, x); + udelay(CLOCK_RATE); +_func_exit_; +} + +u16 wait_eeprom_cmd_done(_adapter *padapter) +{ + u8 x; + u16 i, res = false; +_func_enter_; + standby(padapter); + for (i = 0; i < 200; i++) { + x = rtw_read8(padapter, EE_9346CR); + if (x & _EEDO) { + res = true; + goto exit; + } + udelay(CLOCK_RATE); + } +exit: +_func_exit_; + return res; +} + +void eeprom_clean(_adapter *padapter) +{ + u16 x; +_func_enter_; + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + x &= ~(_EECS | _EEDI); + rtw_write8(padapter, EE_9346CR, (u8)x); + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + up_clk(padapter, &x); + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + down_clk(padapter, &x); +out: +_func_exit_; +} + +void eeprom_write16(_adapter *padapter, u16 reg, u16 data) +{ + u8 x; + +_func_enter_; + + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, x); + + shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5); + + if (padapter->EepromAddressSize == 8) /*CF+ and SDIO*/ + shift_out_bits(padapter, 0, 6); + else /*USB*/ + shift_out_bits(padapter, 0, 4); + + standby(padapter); + +/* Commented out by rcnjko, 2004.0 +* Erase this particular word. Write the erase opcode and register +* number in that order. The opcode is 3bits in length; reg is 6 bits long. +* shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3); +* shift_out_bits(Adapter, reg, Adapter->EepromAddressSize); +* +* if (wait_eeprom_cmd_done(Adapter ) == false) +* { +* return; +* } +*/ + + standby(padapter); + + /* write the new word to the EEPROM*/ + + /* send the write opcode the EEPORM*/ + shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3); + + /* select which word in the EEPROM that we are writing to.*/ + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + /* write the data to the selected EEPROM word.*/ + shift_out_bits(padapter, data, 16); + + if (wait_eeprom_cmd_done(padapter) == false) { + + goto exit; + } + + standby(padapter); + + shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5); + shift_out_bits(padapter, reg, 4); + + eeprom_clean(padapter); +exit: +_func_exit_; + return; +} + +u16 eeprom_read16(_adapter *padapter, u16 reg) /*ReadEEprom*/ +{ + + u16 x; + u16 data = 0; + +_func_enter_; + + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + /* select EEPROM, reset bits, set _EECS*/ + x = rtw_read8(padapter, EE_9346CR); + + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, (unsigned char)x); + + /* write the read opcode and register number in that order*/ + /* The opcode is 3bits in length, reg is 6 bits long*/ + shift_out_bits(padapter, EEPROM_READ_OPCODE, 3); + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + /* Now read the data (16 bits) in from the selected EEPROM word*/ + data = shift_in_bits(padapter); + + eeprom_clean(padapter); +out: +_func_exit_; + return data; + + +} + + + + +/*From even offset*/ +void eeprom_read_sz(_adapter *padapter, u16 reg, u8 *data, u32 sz) +{ + + u16 x, data16; + u32 i; +_func_enter_; + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + /* select EEPROM, reset bits, set _EECS*/ + x = rtw_read8(padapter, EE_9346CR); + + if (padapter->bSurpriseRemoved == true) { + RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true")); + goto out; + } + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, (unsigned char)x); + + /* write the read opcode and register number in that order*/ + /* The opcode is 3bits in length, reg is 6 bits long*/ + shift_out_bits(padapter, EEPROM_READ_OPCODE, 3); + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + + for (i = 0; i < sz; i += 2) { + data16 = shift_in_bits(padapter); + data[i] = data16 & 0xff; + data[i+1] = data16 >> 8; + } + + eeprom_clean(padapter); +out: +_func_exit_; + + + +} + + +/*addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg)*/ +u8 eeprom_read(_adapter *padapter, u32 addr_off, u8 sz, u8 *rbuf) +{ + u8 quotient, remainder, addr_2align_odd; + u16 reg, stmp, i = 0, idx = 0; +_func_enter_; + reg = (u16)(addr_off >> 1); + addr_2align_odd = (u8)(addr_off & 0x1); + + /*read that start at high part: e.g 1,3,5,7,9,...*/ + if (addr_2align_odd) { + stmp = eeprom_read16(padapter, reg); + rbuf[idx++] = (u8) ((stmp>>8)&0xff); /*return hogh-part of the short*/ + reg++; sz--; + } + + quotient = sz >> 1; + remainder = sz & 0x1; + + for (i = 0; i < quotient; i++) { + stmp = eeprom_read16(padapter, reg+i); + rbuf[idx++] = (u8) (stmp&0xff); + rbuf[idx++] = (u8) ((stmp>>8)&0xff); + } + + reg = reg+i; + if (remainder) { /*end of read at lower part of short : 0,2,4,6,...*/ + stmp = eeprom_read16(padapter, reg); + rbuf[idx] = (u8)(stmp & 0xff); + } +_func_exit_; + return true; +} + + + +void read_eeprom_content(_adapter *padapter) +{ + +_func_enter_; + + +_func_exit_; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_efuse.c b/drivers/staging/rtl8723bs/core/rtw_efuse.c new file mode 100644 index 0000000..8e29802 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_efuse.c @@ -0,0 +1,635 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <hal_data.h> +#include <linux/jiffies.h> + + +/*------------------------Define local variable------------------------------*/ +u8 fakeEfuseBank = 0; +u32 fakeEfuseUsedBytes = 0; +u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0}; +u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0}; +u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0}; + +u32 BTEfuseUsedBytes = 0; +u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE]; +u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0}; +u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0}; + +u32 fakeBTEfuseUsedBytes = 0; +u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE]; +u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0}; +u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0}; +/*------------------------Define local variable------------------------------*/ + +/* */ +#define REG_EFUSE_CTRL 0x0030 +#define EFUSE_CTRL REG_EFUSE_CTRL /* E-Fuse Control. */ +/* */ + +bool +Efuse_Read1ByteFromFakeContent( + struct adapter *padapter, + u16 Offset, + u8 *Value); +bool +Efuse_Read1ByteFromFakeContent( + struct adapter *padapter, + u16 Offset, + u8 *Value) +{ + if (Offset >= EFUSE_MAX_HW_SIZE) { + return false; + } + /* DbgPrint("Read fake content, offset = %d\n", Offset); */ + if (fakeEfuseBank == 0) + *Value = fakeEfuseContent[Offset]; + else + *Value = fakeBTEfuseContent[fakeEfuseBank-1][Offset]; + return true; +} + +bool +Efuse_Write1ByteToFakeContent( + struct adapter *padapter, + u16 Offset, + u8 Value); +bool +Efuse_Write1ByteToFakeContent( + struct adapter *padapter, + u16 Offset, + u8 Value) +{ + if (Offset >= EFUSE_MAX_HW_SIZE) { + return false; + } + if (fakeEfuseBank == 0) + fakeEfuseContent[Offset] = Value; + else{ + fakeBTEfuseContent[fakeEfuseBank-1][Offset] = Value; + } + return true; +} + +/*----------------------------------------------------------------------------- + * Function: Efuse_PowerSwitch + * + * Overview: When we want to enable write operation, we should change to + * pwr on state. When we stop write, we should switch to 500k mode + * and disable LDO 2.5V. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/17/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void +Efuse_PowerSwitch( +struct adapter *padapter, +u8 bWrite, +u8 PwrState) +{ + padapter->HalFunc.EfusePowerSwitch(padapter, bWrite, PwrState); +} + +/*----------------------------------------------------------------------------- + * Function: Efuse_GetCurrentSize + * + * Overview: Get current efuse size!!! + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/16/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +u16 +Efuse_GetCurrentSize( + struct adapter *padapter, + u8 efuseType, + bool bPseudoTest) +{ + u16 ret = 0; + + ret = padapter->HalFunc.EfuseGetCurrentSize(padapter, efuseType, bPseudoTest); + + return ret; +} + +/* 11/16/2008 MH Add description. Get current efuse area enabled word!!. */ +u8 +Efuse_CalculateWordCnts(u8 word_en) +{ + u8 word_cnts = 0; + if (!(word_en & BIT(0))) + word_cnts++; /* 0 : write enable */ + if (!(word_en & BIT(1))) + word_cnts++; + if (!(word_en & BIT(2))) + word_cnts++; + if (!(word_en & BIT(3))) + word_cnts++; + return word_cnts; +} + +/* */ +/* Description: */ +/* 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 adapter *Adapter, + u8 efuseType, + u16 _offset, + u16 _size_byte, + u8 *pbuf, +bool bPseudoTest + ); +void +efuse_ReadEFuse( + struct adapter *Adapter, + u8 efuseType, + u16 _offset, + u16 _size_byte, + u8 *pbuf, +bool bPseudoTest + ) +{ + Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest); +} + +void +EFUSE_GetEfuseDefinition( + struct adapter *padapter, + u8 efuseType, + u8 type, + void *pOut, + bool bPseudoTest + ) +{ + padapter->HalFunc.EFUSEGetEfuseDefinition(padapter, efuseType, type, pOut, bPseudoTest); +} + +/*----------------------------------------------------------------------------- + * Function: EFUSE_Read1Byte + * + * Overview: Copy from WMAC fot EFUSE read 1 byte. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 09/23/2008 MHC Copy from WMAC. + * + *---------------------------------------------------------------------------*/ +u8 +EFUSE_Read1Byte( +struct adapter *Adapter, +u16 Address) +{ + u8 data; + u8 Bytetemp = {0x00}; + u8 temp = {0x00}; + u32 k = 0; + u16 contentLen = 0; + + EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&contentLen, false); + + if (Address < contentLen) {/* E-fuse 512Byte */ + /* Write E-fuse Register address bit0~7 */ + temp = Address & 0xFF; + rtw_write8(Adapter, EFUSE_CTRL+1, temp); + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2); + /* Write E-fuse Register address bit8~9 */ + temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC); + rtw_write8(Adapter, EFUSE_CTRL+2, temp); + + /* Write 0x30[31]= 0 */ + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + temp = Bytetemp & 0x7F; + rtw_write8(Adapter, EFUSE_CTRL+3, temp); + + /* Wait Write-ready (0x30[31]= 1) */ + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + while (!(Bytetemp & 0x80)) { + Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3); + k++; + if (k == 1000) { + k = 0; + break; + } + } + data = rtw_read8(Adapter, EFUSE_CTRL); + return data; + } else + return 0xFF; + +} /* EFUSE_Read1Byte */ + +/* 11/16/2008 MH Read one byte from real Efuse. */ +u8 +efuse_OneByteRead( +struct adapter *padapter, +u16 addr, +u8 *data, +bool bPseudoTest) +{ + u32 tmpidx = 0; + u8 bResult; + u8 readbyte; + + /* DBG_871X("===> EFUSE_OneByteRead(), addr = %x\n", addr); */ + /* DBG_871X("===> EFUSE_OneByteRead() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */ + + if (bPseudoTest) { + bResult = Efuse_Read1ByteFromFakeContent(padapter, addr, data); + return bResult; + } + + /* <20130121, Kordan> For SMIC EFUSE specificatoin. */ + /* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */ + /* PHY_SetMacReg(padapter, 0x34, BIT11, 0); */ + rtw_write16(padapter, 0x34, rtw_read16(padapter, 0x34) & (~BIT11)); + + /* -----------------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 */ + /* Write bit 32 0 */ + readbyte = rtw_read8(padapter, EFUSE_CTRL+3); + rtw_write8(padapter, EFUSE_CTRL+3, (readbyte & 0x7f)); + + while (!(0x80 & rtw_read8(padapter, EFUSE_CTRL+3)) && (tmpidx < 1000)) { + mdelay(1); + tmpidx++; + } + if (tmpidx < 100) { + *data = rtw_read8(padapter, EFUSE_CTRL); + bResult = true; + } else{ + *data = 0xff; + bResult = false; + DBG_871X("%s: [ERROR] addr = 0x%x bResult =%d time out 1s !!!\n", __func__, addr, bResult); + DBG_871X("%s: [ERROR] EFUSE_CTRL = 0x%08x !!!\n", __func__, rtw_read32(padapter, EFUSE_CTRL)); + } + + return bResult; +} + +/* 11/16/2008 MH Write one byte to reald Efuse. */ +u8 +efuse_OneByteWrite( +struct adapter *padapter, +u16 addr, +u8 data, +bool bPseudoTest) +{ + u8 tmpidx = 0; + u8 bResult = false; + u32 efuseValue = 0; + + /* DBG_871X("===> EFUSE_OneByteWrite(), addr = %x data =%x\n", addr, data); */ + /* DBG_871X("===> EFUSE_OneByteWrite() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */ + + if (bPseudoTest) { + bResult = Efuse_Write1ByteToFakeContent(padapter, addr, data); + return bResult; + } + + + /* -----------------e-fuse reg ctrl --------------------------------- */ + /* address */ + + + efuseValue = rtw_read32(padapter, EFUSE_CTRL); + efuseValue |= (BIT21|BIT31); + efuseValue &= ~(0x3FFFF); + efuseValue |= ((addr<<8 | data) & 0x3FFFF); + + + /* <20130227, Kordan> 8192E MP chip A-cut had better not set 0x34[11] until B-Cut. */ + + /* <20130121, Kordan> For SMIC EFUSE specificatoin. */ + /* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */ + /* PHY_SetMacReg(padapter, 0x34, BIT11, 1); */ + rtw_write16(padapter, 0x34, rtw_read16(padapter, 0x34) | (BIT11)); + rtw_write32(padapter, EFUSE_CTRL, 0x90600000|((addr<<8 | data))); + + while ((0x80 & rtw_read8(padapter, EFUSE_CTRL+3)) && (tmpidx < 100)) { + mdelay(1); + tmpidx++; + } + + if (tmpidx < 100) { + bResult = true; + } else{ + bResult = false; + DBG_871X("%s: [ERROR] addr = 0x%x , efuseValue = 0x%x , bResult =%d time out 1s !!!\n", + __func__, addr, efuseValue, bResult); + DBG_871X("%s: [ERROR] EFUSE_CTRL = 0x%08x !!!\n", __func__, rtw_read32(padapter, EFUSE_CTRL)); + } + + /* disable Efuse program enable */ + PHY_SetMacReg(padapter, EFUSE_TEST, BIT(11), 0); + + return bResult; +} + +int +Efuse_PgPacketRead(struct adapter *padapter, + u8 offset, + u8 *data, + bool bPseudoTest) +{ + int ret = 0; + + ret = padapter->HalFunc.Efuse_PgPacketRead(padapter, offset, data, bPseudoTest); + + return ret; +} + +int +Efuse_PgPacketWrite(struct adapter *padapter, + u8 offset, + u8 word_en, + u8 *data, + bool bPseudoTest) +{ + int ret; + + ret = padapter->HalFunc.Efuse_PgPacketWrite(padapter, offset, word_en, data, bPseudoTest); + + return ret; +} + +/*----------------------------------------------------------------------------- + * Function: efuse_WordEnableDataRead + * + * Overview: Read allowed word in current efuse section data. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/16/2008 MHC Create Version 0. + * 11/21/2008 MHC Fix Write bug when we only enable late word. + * + *---------------------------------------------------------------------------*/ +void +efuse_WordEnableDataRead(u8 word_en, + u8 *sourdata, + u8 *targetdata) +{ + if (!(word_en&BIT(0))) { + targetdata[0] = sourdata[0]; + targetdata[1] = sourdata[1]; + } + if (!(word_en&BIT(1))) { + targetdata[2] = sourdata[2]; + targetdata[3] = sourdata[3]; + } + if (!(word_en&BIT(2))) { + targetdata[4] = sourdata[4]; + targetdata[5] = sourdata[5]; + } + if (!(word_en&BIT(3))) { + targetdata[6] = sourdata[6]; + targetdata[7] = sourdata[7]; + } +} + + +u8 +Efuse_WordEnableDataWrite(struct adapter *padapter, + u16 efuse_addr, + u8 word_en, + u8 *data, + bool bPseudoTest) +{ + u8 ret = 0; + + ret = padapter->HalFunc.Efuse_WordEnableDataWrite(padapter, efuse_addr, word_en, data, bPseudoTest); + + return ret; +} + +/*----------------------------------------------------------------------------- + * 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 adapter *padapter, + u8 efuseType, + u8 *Efuse, + bool bPseudoTest); +void +Efuse_ReadAllMap( + struct adapter *padapter, + u8 efuseType, + u8 *Efuse, + bool bPseudoTest) +{ + u16 mapLen = 0; + + Efuse_PowerSwitch(padapter, false, true); + + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + + efuse_ReadEFuse(padapter, efuseType, 0, mapLen, Efuse, bPseudoTest); + + Efuse_PowerSwitch(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 adapter *padapter, +u16 Offset, + u8 *Value) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter); + + *Value = pEEPROM->efuse_eeprom_data[Offset]; + +} /* EFUSE_ShadowRead1Byte */ + +/* Read Two Bytes */ +static void +efuse_ShadowRead2Byte( +struct adapter *padapter, +u16 Offset, + u16 *Value) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter); + + *Value = pEEPROM->efuse_eeprom_data[Offset]; + *Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8; + +} /* EFUSE_ShadowRead2Byte */ + +/* Read Four Bytes */ +static void +efuse_ShadowRead4Byte( +struct adapter *padapter, +u16 Offset, + u32 *Value) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter); + + *Value = pEEPROM->efuse_eeprom_data[Offset]; + *Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8; + *Value |= pEEPROM->efuse_eeprom_data[Offset+2]<<16; + *Value |= pEEPROM->efuse_eeprom_data[Offset+3]<<24; + +} /* efuse_ShadowRead4Byte */ + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowMapUpdate + * + * Overview: Transfer current EFUSE content to shadow init and modify map. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/13/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void EFUSE_ShadowMapUpdate( + struct adapter *padapter, + u8 efuseType, + bool bPseudoTest) +{ + struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter); + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + + if (pEEPROM->bautoload_fail_flag == true) { + memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen); + } else{ + Efuse_ReadAllMap(padapter, efuseType, pEEPROM->efuse_eeprom_data, bPseudoTest); + } + + /* PlatformMoveMemory((void *)&pHalData->EfuseMap[EFUSE_MODIFY_MAP][0], */ + /* void *)&pHalData->EfuseMap[EFUSE_INIT_MAP][0], mapLen); */ +} /* EFUSE_ShadowMapUpdate */ + + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowRead + * + * Overview: Read from efuse init map !!!!! + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/12/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void +EFUSE_ShadowRead( + struct adapter *padapter, + u8 Type, + u16 Offset, + u32 *Value) +{ + if (Type == 1) + efuse_ShadowRead1Byte(padapter, Offset, (u8 *)Value); + else if (Type == 2) + efuse_ShadowRead2Byte(padapter, Offset, (u16 *)Value); + else if (Type == 4) + efuse_ShadowRead4Byte(padapter, Offset, (u32 *)Value); + +} /* EFUSE_ShadowRead*/ diff --git a/drivers/staging/rtl8723bs/core/rtw_ieee80211.c b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c new file mode 100644 index 0000000..7b37e08 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c @@ -0,0 +1,1430 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <linux/of.h> + +u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 }; +u16 RTW_WPA_VERSION = 1; +u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 }; +u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 }; +u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 }; + +u16 RSN_VERSION_BSD = 1; +u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 }; +u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 }; +u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 }; +u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 }; +u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 }; +u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 }; +u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 }; +u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 }; +/* */ +/* for adhoc-master to generate ie and provide supported-rate to fw */ +/* */ + +static u8 WIFI_CCKRATES[] = { + (IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK) +}; + +static u8 WIFI_OFDMRATES[] = { + (IEEE80211_OFDM_RATE_6MB), + (IEEE80211_OFDM_RATE_9MB), + (IEEE80211_OFDM_RATE_12MB), + (IEEE80211_OFDM_RATE_18MB), + (IEEE80211_OFDM_RATE_24MB), + IEEE80211_OFDM_RATE_36MB, + IEEE80211_OFDM_RATE_48MB, + IEEE80211_OFDM_RATE_54MB +}; + + +int rtw_get_bit_value_from_ieee_value(u8 val) +{ + unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /* last element must be zero!! */ + + int i = 0; + while (dot11_rate_table[i] != 0) { + if (dot11_rate_table[i] == val) + return BIT(i); + i++; + } + return 0; +} + +uint rtw_is_cckrates_included(u8 *rate) +{ + u32 i = 0; + + while (rate[i] != 0) { + if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) || + (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22)) + return true; + i++; + } + + return false; +} + +uint rtw_is_cckratesonly_included(u8 *rate) +{ + u32 i = 0; + + + while (rate[i] != 0) { + if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) && + (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22)) + return false; + i++; + } + + return true; +} + +int rtw_check_network_type(unsigned char *rate, int ratelen, int channel) +{ + if (channel > 14) { + if ((rtw_is_cckrates_included(rate)) == true) + return WIRELESS_INVALID; + else + return WIRELESS_11A; + } else{ /* could be pure B, pure G, or B/G */ + if ((rtw_is_cckratesonly_included(rate)) == true) + return WIRELESS_11B; + else if ((rtw_is_cckrates_included(rate)) == true) + return WIRELESS_11BG; + else + return WIRELESS_11G; + } + +} + +u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source, + unsigned int *frlen) +{ + memcpy((void *)pbuf, (void *)source, len); + *frlen = *frlen + len; + return (pbuf + len); +} + +/* rtw_set_ie will update frame length */ +u8 *rtw_set_ie +( + u8 *pbuf, + sint index, + uint len, + u8 *source, + uint *frlen /* frame length */ +) +{ + *pbuf = (u8)index; + + *(pbuf + 1) = (u8)len; + + if (len > 0) + memcpy((void *)(pbuf + 2), (void *)source, len); + + *frlen = *frlen + (len + 2); + + return (pbuf + len + 2); +} + +/*---------------------------------------------------------------------------- +index: the information element id index, limit is the limit for search +-----------------------------------------------------------------------------*/ +u8 *rtw_get_ie(u8 *pbuf, sint index, sint *len, sint limit) +{ + sint tmp, i; + u8 *p; + + if (limit < 1) { + return NULL; + } + + p = pbuf; + i = 0; + *len = 0; + while (1) { + if (*p == index) { + *len = *(p + 1); + return p; + } else{ + tmp = *(p + 1); + p += (tmp + 2); + i += (tmp + 2); + } + if (i >= limit) + break; + } + return NULL; +} + +/** + * rtw_get_ie_ex - Search specific IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @eid: Element ID to match + * @oui: OUI to match + * @oui_len: OUI length + * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE + * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE + * + * Returns: The address of the specific IE found, or NULL + */ +u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen) +{ + uint cnt; + u8 *target_ie = NULL; + + + if (ielen) + *ielen = 0; + + if (!in_ie || in_len <= 0) + return target_ie; + + cnt = 0; + + while (cnt < in_len) { + if (eid == in_ie[cnt] + && (!oui || !memcmp(&in_ie[cnt+2], oui, oui_len))) { + target_ie = &in_ie[cnt]; + + if (ie) + memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2); + + if (ielen) + *ielen = in_ie[cnt+1]+2; + + break; + } else{ + cnt += in_ie[cnt+1]+2; /* goto next */ + } + + } + + return target_ie; +} + +/** + * rtw_ies_remove_ie - Find matching IEs and remove + * @ies: Address of IEs to search + * @ies_len: Pointer of length of ies, will update to new length + * @offset: The offset to start scarch + * @eid: Element ID to match + * @oui: OUI to match + * @oui_len: OUI length + * + * Returns: _SUCCESS: ies is updated, _FAIL: not updated + */ +int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len) +{ + int ret = _FAIL; + u8 *target_ie; + u32 target_ielen; + u8 *start; + uint search_len; + + if (!ies || !ies_len || *ies_len <= offset) + goto exit; + + start = ies + offset; + search_len = *ies_len - offset; + + while (1) { + target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen); + if (target_ie && target_ielen) { + u8 buf[MAX_IE_SZ] = {0}; + u8 *remain_ies = target_ie + target_ielen; + uint remain_len = search_len - (remain_ies - start); + + memcpy(buf, remain_ies, remain_len); + memcpy(target_ie, buf, remain_len); + *ies_len = *ies_len - target_ielen; + ret = _SUCCESS; + + start = target_ie; + search_len = remain_len; + } else { + break; + } + } +exit: + return ret; +} + +void rtw_set_supported_rate(u8 *SupportedRates, uint mode) +{ + memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX); + + switch (mode) { + case WIRELESS_11B: + memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN); + break; + + case WIRELESS_11G: + case WIRELESS_11A: + case WIRELESS_11_5N: + case WIRELESS_11A_5N:/* Todo: no basic rate for ofdm ? */ + case WIRELESS_11_5AC: + memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN); + break; + + case WIRELESS_11BG: + case WIRELESS_11G_24N: + case WIRELESS_11_24N: + case WIRELESS_11BG_24N: + memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN); + memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN); + break; + + } +} + +uint rtw_get_rateset_len(u8 *rateset) +{ + uint i = 0; + + while (1) { + if ((rateset[i]) == 0) + break; + + if (i > 12) + break; + + i++; + } + return i; +} + +int rtw_generate_ie(struct registry_priv *pregistrypriv) +{ + u8 wireless_mode; + int sz = 0, rateLen; + struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network; + u8 *ie = pdev_network->IEs; + + /* timestamp will be inserted by hardware */ + sz += 8; + ie += sz; + + /* beacon interval : 2bytes */ + *(__le16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);/* BCN_INTERVAL; */ + sz += 2; + ie += 2; + + /* capability info */ + *(u16 *)ie = 0; + + *(__le16 *)ie |= cpu_to_le16(cap_IBSS); + + if (pregistrypriv->preamble == PREAMBLE_SHORT) + *(__le16 *)ie |= cpu_to_le16(cap_ShortPremble); + + if (pdev_network->Privacy) + *(__le16 *)ie |= cpu_to_le16(cap_Privacy); + + sz += 2; + ie += 2; + + /* SSID */ + ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz); + + /* supported rates */ + if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) { + if (pdev_network->Configuration.DSConfig > 14) + wireless_mode = WIRELESS_11A_5N; + else + wireless_mode = WIRELESS_11BG_24N; + } else{ + wireless_mode = pregistrypriv->wireless_mode; + } + + rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode); + + rateLen = rtw_get_rateset_len(pdev_network->SupportedRates); + + if (rateLen > 8) { + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz); + /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */ + } else{ + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz); + } + + /* DS parameter set */ + ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz); + + + /* IBSS Parameter Set */ + + ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz); + + if (rateLen > 8) { + ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); + } + + /* HT Cap. */ + if (((pregistrypriv->wireless_mode&WIRELESS_11_5N) || (pregistrypriv->wireless_mode&WIRELESS_11_24N)) + && (pregistrypriv->ht_enable == true)) { + /* todo: */ + } + + /* pdev_network->IELength = sz; update IELength */ + + /* return _SUCCESS; */ + + return sz; +} + +unsigned char *rtw_get_wpa_ie(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; + __le16 le_tmp; + + while (1) { + pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new); + + if (pbuf) { + + /* check if oui matches... */ + if (memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type))) { + + goto check_next_ie; + } + + /* check version... */ + memcpy((u8 *)&le_tmp, (pbuf + 6), sizeof(val16)); + + val16 = le16_to_cpu(le_tmp); + if (val16 != 0x0001) + goto check_next_ie; + + *wpa_ie_len = *(pbuf + 1); + + return pbuf; + + } else{ + + *wpa_ie_len = 0; + return NULL; + } + +check_next_ie: + + limit_new = limit - (pbuf - pie) - 2 - len; + + if (limit_new <= 0) + break; + + pbuf += (2 + len); + + } + + *wpa_ie_len = 0; + + return NULL; + +} + +unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit) +{ + + return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit); + +} + +int rtw_get_wpa_cipher_suite(u8 *s) +{ + if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN)) + return WPA_CIPHER_NONE; + if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN)) + return WPA_CIPHER_WEP40; + if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN)) + return WPA_CIPHER_TKIP; + if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN)) + return WPA_CIPHER_CCMP; + if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN)) + return WPA_CIPHER_WEP104; + + return 0; +} + +int rtw_get_wpa2_cipher_suite(u8 *s) +{ + if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN)) + return WPA_CIPHER_NONE; + if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN)) + return WPA_CIPHER_WEP40; + if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN)) + return WPA_CIPHER_TKIP; + if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN)) + return WPA_CIPHER_CCMP; + if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN)) + return WPA_CIPHER_WEP104; + + return 0; +} + + +int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x) +{ + int i, ret = _SUCCESS; + int left, count; + u8 *pos; + u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1}; + + if (wpa_ie_len <= 0) { + /* No WPA IE - fail silently */ + return _FAIL; + } + + + if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) || + (memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN))) { + return _FAIL; + } + + pos = wpa_ie; + + pos += 8; + left = wpa_ie_len - 8; + + + /* group_cipher */ + if (left >= WPA_SELECTOR_LEN) { + + *group_cipher = rtw_get_wpa_cipher_suite(pos); + + pos += WPA_SELECTOR_LEN; + left -= WPA_SELECTOR_LEN; + + } else if (left > 0) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left)); + + return _FAIL; + } + + + /* pairwise_cipher */ + if (left >= 2) { + /* count = 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_suite(pos); + + pos += WPA_SELECTOR_LEN; + left -= WPA_SELECTOR_LEN; + } + + } else if (left == 1) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)", __func__)); + return _FAIL; + } + + if (is_8021x) { + if (left >= 6) { + pos += 2; + if (!memcmp(pos, SUITE_1X, 4)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s : there has 802.1x auth\n", __func__)); + *is_8021x = 1; + } + } + } + + return ret; + +} + +int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x) +{ + int i, ret = _SUCCESS; + int left, count; + u8 *pos; + u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01}; + + if (rsn_ie_len <= 0) { + /* No RSN IE - fail silently */ + return _FAIL; + } + + + if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2))) { + return _FAIL; + } + + pos = rsn_ie; + pos += 4; + left = rsn_ie_len - 4; + + /* group_cipher */ + if (left >= RSN_SELECTOR_LEN) { + + *group_cipher = rtw_get_wpa2_cipher_suite(pos); + + pos += RSN_SELECTOR_LEN; + left -= RSN_SELECTOR_LEN; + + } else if (left > 0) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left)); + return _FAIL; + } + + /* pairwise_cipher */ + if (left >= 2) { + /* count = 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_suite(pos); + + pos += RSN_SELECTOR_LEN; + left -= RSN_SELECTOR_LEN; + } + + } else if (left == 1) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)", __func__)); + + return _FAIL; + } + + if (is_8021x) { + if (left >= 6) { + pos += 2; + if (!memcmp(pos, SUITE_1X, 4)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__)); + *is_8021x = 1; + } + } + } + + return ret; + +} + +/* ifdef CONFIG_WAPI_SUPPORT */ +int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len) +{ + int len = 0; + u8 authmode, i; + uint cnt; + u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01}; + u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02}; + + if (wapi_len) + *wapi_len = 0; + + if (!in_ie || in_len <= 0) + return len; + + cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_); + + while (cnt < in_len) { + authmode = in_ie[cnt]; + + /* if (authmode == _WAPI_IE_) */ + if (authmode == _WAPI_IE_ && (!memcmp(&in_ie[cnt+6], wapi_oui1, 4) || + !memcmp(&in_ie[cnt+6], wapi_oui2, 4))) { + if (wapi_ie) { + memcpy(wapi_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", + wapi_ie[i], wapi_ie[i+1], wapi_ie[i+2], wapi_ie[i+3], wapi_ie[i+4], + wapi_ie[i+5], wapi_ie[i+6], wapi_ie[i+7])); + } + } + + if (wapi_len) + *wapi_len = in_ie[cnt+1]+2; + + cnt += in_ie[cnt+1]+2; /* get next */ + } else{ + cnt += in_ie[cnt+1]+2; /* get next */ + } + } + + if (wapi_len) + len = *wapi_len; + + return len; +} +/* endif */ + +int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len) +{ + u8 authmode, sec_idx, i; + u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01}; + uint cnt; + + /* Search required WPA or WPA2 IE and copy to sec_ie[ ] */ + + cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_); + + sec_idx = 0; + + while (cnt < in_len) { + authmode = in_ie[cnt]; + + if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], &wpa_oui[0], 4))) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_get_wpa_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n", sec_idx, in_ie[cnt+1]+2)); + + if (wpa_ie) { + memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2); + + for (i = 0; i < (in_ie[cnt+1]+2); i = 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_ie(u8 *ie_ptr, uint *wps_ielen) +{ + u8 match = false; + u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04}; + + if (ie_ptr == NULL) + return match; + + eid = ie_ptr[0]; + + if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) { + /* DBG_8192C("==> found WPS_IE.....\n"); */ + *wps_ielen = ie_ptr[1]+2; + match = true; + } + return match; +} + +/** + * rtw_get_wps_ie - Search WPS IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie + * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE + * + * Returns: The address of the WPS IE found, or NULL + */ +u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen) +{ + uint cnt; + u8 *wpsie_ptr = NULL; + u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04}; + + if (wps_ielen) + *wps_ielen = 0; + + if (!in_ie || in_len <= 0) + return wpsie_ptr; + + cnt = 0; + + while (cnt < in_len) { + eid = in_ie[cnt]; + + if ((eid == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], wps_oui, 4))) { + wpsie_ptr = &in_ie[cnt]; + + if (wps_ie) + memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2); + + if (wps_ielen) + *wps_ielen = in_ie[cnt+1]+2; + + cnt += in_ie[cnt+1]+2; + + break; + } else{ + cnt += in_ie[cnt+1]+2; /* goto next */ + } + + } + + return wpsie_ptr; +} + +/** + * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE + * @wps_ie: Address of WPS IE to search + * @wps_ielen: Length limit from wps_ie + * @target_attr_id: The attribute ID of WPS attribute to search + * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr + * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute + * + * Returns: the address of the specific WPS attribute found, or NULL + */ +u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr) +{ + u8 *attr_ptr = NULL; + u8 *target_attr_ptr = NULL; + u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04}; + + if (len_attr) + *len_attr = 0; + + if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) || + (memcmp(wps_ie + 2, wps_oui, 4))) { + return attr_ptr; + } + + /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */ + attr_ptr = wps_ie + 6; /* goto first attr */ + + while (attr_ptr - wps_ie < wps_ielen) { + /* 4 = 2(Attribute ID) + 2(Length) */ + u16 attr_id = RTW_GET_BE16(attr_ptr); + u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2); + u16 attr_len = attr_data_len + 4; + + /* DBG_871X("%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_content - Search a specific WPS attribute content from a given WPS IE + * @wps_ie: Address of WPS IE to search + * @wps_ielen: Length limit from wps_ie + * @target_attr_id: The attribute ID of WPS attribute to search + * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content + * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content + * + * Returns: the address of the specific WPS attribute content found, or NULL + */ +u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content) +{ + u8 *attr_ptr; + u32 attr_len; + + if (len_content) + *len_content = 0; + + attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len); + + if (attr_ptr && attr_len) { + if (buf_content) + memcpy(buf_content, attr_ptr+4, attr_len-4); + + if (len_content) + *len_content = attr_len-4; + + return attr_ptr+4; + } + + return NULL; +} + +static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen, + struct rtw_ieee802_11_elems *elems, + int show_errors) +{ + unsigned int oui; + + /* first 3 bytes in vendor specific information element are the IEEE + * OUI of the vendor. The following byte is used a vendor specific + * sub-type. */ + if (elen < 4) { + if (show_errors) { + DBG_871X("short vendor specific " + "information element ignored (len =%lu)\n", + (unsigned long) elen); + } + return -1; + } + + oui = RTW_GET_BE24(pos); + switch (oui) { + case OUI_MICROSOFT: + /* Microsoft/Wi-Fi information elements are further typed and + * subtyped */ + switch (pos[3]) { + case 1: + /* Microsoft OUI (00:50:F2) with OUI Type 1: + * real WPA information element */ + elems->wpa_ie = pos; + elems->wpa_ie_len = elen; + break; + case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */ + if (elen < 5) { + DBG_871X("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_871X("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_871X("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_871X("Unknown Broadcom " + "information element ignored " + "(type =%d len =%lu)\n", + pos[3], (unsigned long) elen); + return -1; + } + break; + + default: + DBG_871X("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 + */ +ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len, + struct rtw_ieee802_11_elems *elems, + int show_errors) +{ + uint left = len; + u8 *pos = start; + int unknown = 0; + + memset(elems, 0, sizeof(*elems)); + + while (left >= 2) { + u8 id, elen; + + id = *pos++; + elen = *pos++; + left -= 2; + + if (elen > left) { + if (show_errors) { + DBG_871X("IEEE 802.11 element " + "parse failed (id =%d elen =%d " + "left =%lu)\n", + id, elen, (unsigned long) left); + } + return ParseFailed; + } + + switch (id) { + case WLAN_EID_SSID: + elems->ssid = pos; + elems->ssid_len = elen; + break; + case WLAN_EID_SUPP_RATES: + elems->supp_rates = pos; + elems->supp_rates_len = elen; + break; + case WLAN_EID_FH_PARAMS: + elems->fh_params = pos; + elems->fh_params_len = elen; + break; + case WLAN_EID_DS_PARAMS: + elems->ds_params = pos; + elems->ds_params_len = elen; + break; + case WLAN_EID_CF_PARAMS: + elems->cf_params = pos; + elems->cf_params_len = elen; + break; + case WLAN_EID_TIM: + elems->tim = pos; + elems->tim_len = elen; + break; + case WLAN_EID_IBSS_PARAMS: + elems->ibss_params = pos; + elems->ibss_params_len = elen; + break; + case WLAN_EID_CHALLENGE: + elems->challenge = pos; + elems->challenge_len = elen; + break; + case WLAN_EID_ERP_INFO: + elems->erp_info = pos; + elems->erp_info_len = elen; + break; + case WLAN_EID_EXT_SUPP_RATES: + elems->ext_supp_rates = pos; + elems->ext_supp_rates_len = elen; + break; + case WLAN_EID_VENDOR_SPECIFIC: + if (rtw_ieee802_11_parse_vendor_specific(pos, elen, + elems, + show_errors)) + unknown++; + break; + case WLAN_EID_RSN: + elems->rsn_ie = pos; + elems->rsn_ie_len = elen; + break; + case WLAN_EID_PWR_CAPABILITY: + elems->power_cap = pos; + elems->power_cap_len = elen; + break; + case WLAN_EID_SUPPORTED_CHANNELS: + elems->supp_channels = pos; + elems->supp_channels_len = elen; + break; + case WLAN_EID_MOBILITY_DOMAIN: + elems->mdie = pos; + elems->mdie_len = elen; + break; + case WLAN_EID_FAST_BSS_TRANSITION: + elems->ftie = pos; + elems->ftie_len = elen; + break; + case WLAN_EID_TIMEOUT_INTERVAL: + elems->timeout_int = pos; + elems->timeout_int_len = elen; + break; + case WLAN_EID_HT_CAP: + elems->ht_capabilities = pos; + elems->ht_capabilities_len = elen; + break; + case WLAN_EID_HT_OPERATION: + elems->ht_operation = pos; + elems->ht_operation_len = elen; + break; + case WLAN_EID_VHT_CAPABILITY: + elems->vht_capabilities = pos; + elems->vht_capabilities_len = elen; + break; + case WLAN_EID_VHT_OPERATION: + elems->vht_operation = pos; + elems->vht_operation_len = elen; + break; + case WLAN_EID_VHT_OP_MODE_NOTIFY: + elems->vht_op_mode_notify = pos; + elems->vht_op_mode_notify_len = elen; + break; + default: + unknown++; + if (!show_errors) + break; + DBG_871X("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); +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 key_2char2num(u8 hch, u8 lch); +u8 key_2char2num(u8 hch, u8 lch) +{ + return ((key_char2num(hch) << 4) | key_char2num(lch)); +} + +void rtw_macaddr_cfg(struct device *dev, u8 *mac_addr) +{ + u8 mac[ETH_ALEN]; + struct device_node *np = dev->of_node; + const unsigned char *addr; + int len; + + if (mac_addr == NULL) + return; + + if (rtw_initmac) { /* Users specify the mac address */ + int jj, kk; + + for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3) { + mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]); + } + memcpy(mac_addr, mac, ETH_ALEN); + } else{ /* Use the mac address stored in the Efuse */ + memcpy(mac, mac_addr, ETH_ALEN); + } + + if (((mac[0] == 0xff) && (mac[1] == 0xff) && (mac[2] == 0xff) && + (mac[3] == 0xff) && (mac[4] == 0xff) && (mac[5] == 0xff)) || + ((mac[0] == 0x00) && (mac[1] == 0x00) && (mac[2] == 0x00) && + (mac[3] == 0x00) && (mac[4] == 0x00) && (mac[5] == 0x00))) { + if (np && + (addr = of_get_property(np, "local-mac-address", &len)) && + len == ETH_ALEN) { + memcpy(mac_addr, addr, ETH_ALEN); + } else { + 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_871X("MAC Address from efuse error, assign default one !!!\n"); + } + } + + DBG_871X("rtw_macaddr_cfg MAC Address = "MAC_FMT"\n", MAC_ARG(mac_addr)); +} + +static int rtw_get_cipher_info(struct wlan_network *pnetwork) +{ + u32 wpa_ielen; + unsigned char *pbuf; + int group_cipher = 0, pairwise_cipher = 0, is8021x = 0; + int ret = _FAIL; + pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12); + + if (pbuf && (wpa_ielen > 0)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen)); + if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) { + + pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher; + pnetwork->BcnInfo.group_cipher = group_cipher; + pnetwork->BcnInfo.is_8021x = is8021x; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d", + __func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x)); + ret = _SUCCESS; + } + } else { + + pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12); + + if (pbuf && (wpa_ielen > 0)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE\n")); + if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE OK!!!\n")); + pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher; + pnetwork->BcnInfo.group_cipher = group_cipher; + pnetwork->BcnInfo.is_8021x = is8021x; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d," + "pnetwork->group_cipher is %d, is_8021x is %d", __func__, pnetwork->BcnInfo.pairwise_cipher, + pnetwork->BcnInfo.group_cipher, pnetwork->BcnInfo.is_8021x)); + ret = _SUCCESS; + } + } + } + + return ret; +} + +void rtw_get_bcn_info(struct wlan_network *pnetwork) +{ + unsigned short cap = 0; + u8 bencrypt = 0; + /* 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; + __le16 le_cap; + + memcpy((u8 *)&le_cap, rtw_get_capability_from_ie(pnetwork->network.IEs), 2); + cap = le16_to_cpu(le_cap); + if (cap & WLAN_CAPABILITY_PRIVACY) { + bencrypt = 1; + pnetwork->network.Privacy = 1; + } else { + pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS; + } + rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len)); + + if (rsn_len > 0) { + pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2; + } else if (wpa_len > 0) { + pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA; + } else { + if (bencrypt) + pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP; + } + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n", + pnetwork->BcnInfo.encryp_protocol)); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n", + pnetwork->BcnInfo.encryp_protocol)); + rtw_get_cipher_info(pnetwork); + + /* get bwmode and ch_offset */ + /* parsing HT_CAP_IE */ + p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_); + if (p && len > 0) { + pht_cap = (struct ieee80211_ht_cap *)(p + 2); + pnetwork->BcnInfo.ht_cap_info = le16_to_cpu(pht_cap->cap_info); + } else { + pnetwork->BcnInfo.ht_cap_info = 0; + } + /* parsing HT_INFO_IE */ + p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_); + if (p && len > 0) { + pht_info = (struct HT_info_element *)(p + 2); + pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0]; + } else { + pnetwork->BcnInfo.ht_info_infos_0 = 0; + } +} + +/* show MCS rate, unit: 100Kbps */ +u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI, 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)?1500:1350):((short_GI)?722:650); + else if (MCS_rate[0] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI)?1350:1215):((short_GI)?650:585); + else if (MCS_rate[0] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520); + else if (MCS_rate[0] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390); + else if (MCS_rate[0] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260); + else if (MCS_rate[0] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI)?450:405):((short_GI)?217:195); + else if (MCS_rate[0] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130); + else if (MCS_rate[0] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65); + } else{ + if (MCS_rate[1]) { + if (MCS_rate[1] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI)?3000:2700):((short_GI)?1444:1300); + else if (MCS_rate[1] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI)?2700:2430):((short_GI)?1300:1170); + else if (MCS_rate[1] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI)?2400:2160):((short_GI)?1156:1040); + else if (MCS_rate[1] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI)?1800:1620):((short_GI)?867:780); + else if (MCS_rate[1] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520); + else if (MCS_rate[1] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390); + else if (MCS_rate[1] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260); + else if (MCS_rate[1] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130); + } else{ + if (MCS_rate[0] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI)?1500:1350):((short_GI)?722:650); + else if (MCS_rate[0] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI)?1350:1215):((short_GI)?650:585); + else if (MCS_rate[0] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520); + else if (MCS_rate[0] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390); + else if (MCS_rate[0] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260); + else if (MCS_rate[0] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI)?450:405):((short_GI)?217:195); + else if (MCS_rate[0] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130); + else if (MCS_rate[0] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65); + } + } + return max_rate; +} + +int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action) +{ + const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr); + u16 fc; + u8 c; + u8 a = ACT_PUBLIC_MAX; + + fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control); + + if ((fc & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE)) + != (RTW_IEEE80211_FTYPE_MGMT|RTW_IEEE80211_STYPE_ACTION) + ) { + return false; + } + + c = frame_body[0]; + + switch (c) { + case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */ + break; + default: + a = frame_body[1]; + } + + if (category) + *category = c; + if (action) + *action = a; + + return true; +} + +static const char *_action_public_str[] = { + "ACT_PUB_BSSCOEXIST", + "ACT_PUB_DSE_ENABLE", + "ACT_PUB_DSE_DEENABLE", + "ACT_PUB_DSE_REG_LOCATION", + "ACT_PUB_EXT_CHL_SWITCH", + "ACT_PUB_DSE_MSR_REQ", + "ACT_PUB_DSE_MSR_RPRT", + "ACT_PUB_MP", + "ACT_PUB_DSE_PWR_CONSTRAINT", + "ACT_PUB_VENDOR", + "ACT_PUB_GAS_INITIAL_REQ", + "ACT_PUB_GAS_INITIAL_RSP", + "ACT_PUB_GAS_COMEBACK_REQ", + "ACT_PUB_GAS_COMEBACK_RSP", + "ACT_PUB_TDLS_DISCOVERY_RSP", + "ACT_PUB_LOCATION_TRACK", + "ACT_PUB_RSVD", +}; + +const char *action_public_str(u8 action) +{ + action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action; + return _action_public_str[action]; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_io.c b/drivers/staging/rtl8723bs/core/rtw_io.c new file mode 100644 index 0000000..6bd5a474 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_io.c @@ -0,0 +1,203 @@ +/****************************************************************************** + * + * 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. CONFIG_SDIO_HCI: + a. USE_SYNC_IRP: Only sync operations are provided. + b. USE_ASYNC_IRP:Both sync/async operations are provided. + +jackson@realtek.com.tw + +*/ + +#define _RTW_IO_C_ + +#include <drv_types.h> +#include <rtw_debug.h> + +#define rtw_le16_to_cpu(val) val +#define rtw_le32_to_cpu(val) val +#define rtw_cpu_to_le16(val) val +#define rtw_cpu_to_le32(val) val + +u8 _rtw_read8(struct adapter *adapter, u32 addr) +{ + u8 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr); + + _read8 = pintfhdl->io_ops._read8; + + r_val = _read8(pintfhdl, addr); + return r_val; +} + +u16 _rtw_read16(struct adapter *adapter, u32 addr) +{ + u16 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr); + + _read16 = pintfhdl->io_ops._read16; + + r_val = _read16(pintfhdl, addr); + return rtw_le16_to_cpu(r_val); +} + +u32 _rtw_read32(struct adapter *adapter, u32 addr) +{ + u32 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr); + + _read32 = pintfhdl->io_ops._read32; + + r_val = _read32(pintfhdl, addr); + return rtw_le32_to_cpu(r_val); + +} + +int _rtw_write8(struct adapter *adapter, u32 addr, u8 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val); + int ret; + + _write8 = pintfhdl->io_ops._write8; + + ret = _write8(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} +int _rtw_write16(struct adapter *adapter, u32 addr, u16 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val); + int ret; + + _write16 = pintfhdl->io_ops._write16; + + ret = _write16(pintfhdl, addr, val); + return RTW_STATUS_CODE(ret); +} +int _rtw_write32(struct adapter *adapter, u32 addr, u32 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val); + int ret; + + _write32 = pintfhdl->io_ops._write32; + + ret = _write32(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} + +u8 _rtw_sd_f0_read8(struct adapter *adapter, u32 addr) +{ + u8 r_val = 0x00; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u8 (*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr); + + _sd_f0_read8 = pintfhdl->io_ops._sd_f0_read8; + + if (_sd_f0_read8) + r_val = _sd_f0_read8(pintfhdl, addr); + else + DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" _sd_f0_read8 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return r_val; +} + +u32 _rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem); + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u32 ret = _SUCCESS; + + _write_port = pintfhdl->io_ops._write_port; + + ret = _write_port(pintfhdl, addr, cnt, pmem); + + return ret; +} + +int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct adapter *padapter, 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(padapter, &pintf->io_ops); + + return _SUCCESS; +} + +/* +* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR +* @return true: +* @return false: +*/ +int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj) +{ + int ret = false; + int value = atomic_inc_return(&dvobj->continual_io_error); + if (value > MAX_CONTINUAL_IO_ERR) { + DBG_871X("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR); + ret = true; + } else { + /* DBG_871X("[dvobj:%p] continual_io_error:%d\n", dvobj, value); */ + } + return ret; +} + +/* +* Set the continual_io_error of this @param dvobjprive to 0 +*/ +void rtw_reset_continual_io_error(struct dvobj_priv *dvobj) +{ + atomic_set(&dvobj->continual_io_error, 0); +} diff --git a/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c b/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c new file mode 100644 index 0000000..e0793f8 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c @@ -0,0 +1,698 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> + +#define IS_MAC_ADDRESS_BROADCAST(addr) \ +(\ + ((addr[0] == 0xff) && (addr[1] == 0xff) && \ + (addr[2] == 0xff) && (addr[3] == 0xff) && \ + (addr[4] == 0xff) && (addr[5] == 0xff)) ? true : false \ +) + +u8 rtw_validate_bssid(u8 *bssid) +{ + u8 ret = true; + + if (is_zero_mac_addr(bssid) + || is_broadcast_mac_addr(bssid) + || is_multicast_mac_addr(bssid) + ) { + ret = false; + } + + return ret; +} + +u8 rtw_validate_ssid(struct ndis_802_11_ssid *ssid) +{ + u8 ret = true; + + if (ssid->SsidLength > 32) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid length >32\n")); + ret = false; + goto exit; + } + +#ifdef CONFIG_VALIDATE_SSID + for (i = 0; i < ssid->SsidLength; i++) { + /* wifi, printable ascii code must be supported */ + if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e))) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid has nonprintabl ascii\n")); + ret = false; + break; + } + } +#endif /* CONFIG_VALIDATE_SSID */ + +exit: + return ret; +} + +u8 rtw_do_join(struct adapter *padapter); +u8 rtw_do_join(struct adapter *padapter) +{ + struct list_head *plist, *phead; + u8 *pibss = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct __queue *queue = &(pmlmepriv->scanned_queue); + u8 ret = _SUCCESS; + + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + phead = get_list_head(queue); + plist = get_next(phead); + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("\n rtw_do_join: phead = %p; plist = %p\n\n\n", phead, plist)); + + pmlmepriv->cur_network.join_res = -2; + + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + + pmlmepriv->pscanned = plist; + + pmlmepriv->to_join = true; + + if (list_empty(&queue->queue)) { + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + + /* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */ + /* we try to issue sitesurvey firstly */ + + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false + || rtw_to_roam(padapter) > 0 + ) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_do_join(): site survey if scanned_queue is empty\n.")); + /* submit site_survey_cmd */ + ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0); + if (_SUCCESS != ret) { + pmlmepriv->to_join = false; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_do_join(): site survey return error\n.")); + } + } else{ + pmlmepriv->to_join = false; + ret = _FAIL; + } + + goto exit; + } else{ + int select_ret; + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv); + if (select_ret == _SUCCESS) { + pmlmepriv->to_join = false; + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + } else{ + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) { + /* submit createbss_cmd to change to a ADHOC_MASTER */ + + /* pmlmepriv->lock has been acquired by caller... */ + struct wlan_bssid_ex *pdev_network = &(padapter->registrypriv.dev_network); + + pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE; + + pibss = padapter->registrypriv.dev_network.MacAddress; + + memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid)); + memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid)); + + rtw_update_registrypriv_dev_network(padapter); + + rtw_generate_random_ibss(pibss); + + if (rtw_createbss_cmd(padapter) != _SUCCESS) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>do_goin: rtw_createbss_cmd status FAIL***\n ")); + ret = false; + goto exit; + } + + pmlmepriv->to_join = false; + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("***Error => rtw_select_and_join_from_scanned_queue FAIL under STA_Mode***\n ")); + + } else{ + /* can't associate ; reset under-linking */ + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + + /* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */ + /* we try to issue sitesurvey firstly */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false + || rtw_to_roam(padapter) > 0 + ) { + /* DBG_871X("rtw_do_join() when no desired bss in scanning queue\n"); */ + ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0); + if (_SUCCESS != ret) { + pmlmepriv->to_join = false; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("do_join(): site survey return error\n.")); + } + } else{ + ret = _FAIL; + pmlmepriv->to_join = false; + } + } + + } + + } + +exit: + return ret; +} + +u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid) +{ + u8 status = _SUCCESS; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + DBG_871X_LEVEL(_drv_always_, "set bssid:%pM\n", bssid); + + if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 && bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) || + (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF && bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) { + status = _FAIL; + goto exit; + } + + spin_lock_bh(&pmlmepriv->lock); + + + DBG_871X("Set BSSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv)); + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) { + goto handle_tkip_countermeasure; + } else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == 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_bssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n")); + + if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN)) { + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false) + goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */ + } else { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set BSSID not the same bssid\n")); + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid ="MAC_FMT"\n", MAC_ARG(bssid))); + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("cur_bssid ="MAC_FMT"\n", MAC_ARG(pmlmepriv->cur_network.network.MacAddress))); + + rtw_disassoc_cmd(padapter, 0, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + rtw_indicate_disconnect(padapter); + + rtw_free_assoc_resources(padapter, 1); + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } + } + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid)); + memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN); + pmlmepriv->assoc_by_bssid = true; + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) { + pmlmepriv->to_join = true; + } else { + status = rtw_do_join(padapter); + } + +release_mlme_lock: + spin_unlock_bh(&pmlmepriv->lock); + +exit: + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("rtw_set_802_11_bssid: status =%d\n", status)); + + return status; +} + +u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid) +{ + u8 status = _SUCCESS; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *pnetwork = &pmlmepriv->cur_network; + + DBG_871X_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_871X("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.SsidLength == ssid->SsidLength) && + (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength))) { + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("Set SSID is the same ssid, fw_state = 0x%08x\n", + get_fwstate(pmlmepriv))); + + if (rtw_is_same_ibss(padapter, pnetwork) == false) { + /* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */ + rtw_disassoc_cmd(padapter, 0, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + rtw_indicate_disconnect(padapter); + + rtw_free_assoc_resources(padapter, 1); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } else{ + goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */ + } + } else { + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1); + } + } else{ + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set SSID not the same ssid\n")); + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_ssid =[%s] len = 0x%x\n", ssid->Ssid, (unsigned int)ssid->SsidLength)); + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("assoc_ssid =[%s] len = 0x%x\n", pmlmepriv->assoc_ssid.Ssid, (unsigned int)pmlmepriv->assoc_ssid.SsidLength)); + + rtw_disassoc_cmd(padapter, 0, true); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + rtw_indicate_disconnect(padapter); + + rtw_free_assoc_resources(padapter, 1); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } + } + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + if (rtw_validate_ssid(ssid) == false) { + status = _FAIL; + goto release_mlme_lock; + } + + memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid)); + pmlmepriv->assoc_by_bssid = false; + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) { + pmlmepriv->to_join = true; + } else { + status = rtw_do_join(padapter); + } + +release_mlme_lock: + spin_unlock_bh(&pmlmepriv->lock); + +exit: + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, + ("-rtw_set_802_11_ssid: status =%d\n", status)); + + return status; +} + +u8 rtw_set_802_11_connect(struct adapter *padapter, u8 *bssid, struct ndis_802_11_ssid *ssid) +{ + u8 status = _SUCCESS; + bool bssid_valid = true; + bool ssid_valid = true; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (!ssid || rtw_validate_ssid(ssid) == false) + ssid_valid = false; + + if (!bssid || rtw_validate_bssid(bssid) == false) + bssid_valid = false; + + if (ssid_valid == false && bssid_valid == false) { + DBG_871X(FUNC_ADPT_FMT" ssid:%p, ssid_valid:%d, bssid:%p, bssid_valid:%d\n", + FUNC_ADPT_ARG(padapter), ssid, ssid_valid, bssid, bssid_valid); + status = _FAIL; + goto exit; + } + + 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_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" fw_state = 0x%08x\n", + FUNC_ADPT_ARG(padapter), 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; + } + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + if (ssid && ssid_valid) + memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct ndis_802_11_ssid)); + else + memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid)); + + if (bssid && bssid_valid) { + memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN); + pmlmepriv->assoc_by_bssid = true; + } else { + pmlmepriv->assoc_by_bssid = false; + } + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) { + pmlmepriv->to_join = true; + } else { + status = rtw_do_join(padapter); + } + +release_mlme_lock: + spin_unlock_bh(&pmlmepriv->lock); + +exit: + return status; +} + +u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter, + enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *cur_network = &pmlmepriv->cur_network; + enum NDIS_802_11_NETWORK_INFRASTRUCTURE *pold_state = &(cur_network->network.InfrastructureMode); + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_, + ("+rtw_set_802_11_infrastructure_mode: old =%d new =%d fw_state = 0x%08x\n", + *pold_state, networktype, get_fwstate(pmlmepriv))); + + if (*pold_state != networktype) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!")); + /* DBG_871X("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; + + stop_ap_mode(padapter); + } + + spin_lock_bh(&pmlmepriv->lock); + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) || (*pold_state == Ndis802_11IBSS)) + rtw_disassoc_cmd(padapter, 0, true); + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) + rtw_free_assoc_resources(padapter, 1); + + if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) { + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + rtw_indicate_disconnect(padapter); /* will clr Linked_state; before this function, we must have chked whether issue dis-assoc_cmd or not */ + } + } + + *pold_state = networktype; + + _clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE); + + switch (networktype) { + case Ndis802_11IBSS: + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + break; + + case Ndis802_11Infrastructure: + set_fwstate(pmlmepriv, WIFI_STATION_STATE); + break; + + case Ndis802_11APMode: + set_fwstate(pmlmepriv, WIFI_AP_STATE); + start_ap_mode(padapter); + /* rtw_indicate_connect(padapter); */ + + 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_disassociate(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + spin_lock_bh(&pmlmepriv->lock); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_disassociate: rtw_indicate_disconnect\n")); + + rtw_disassoc_cmd(padapter, 0, true); + rtw_indicate_disconnect(padapter); + /* modify for CONFIG_IEEE80211W, none 11w can use it */ + rtw_free_assoc_resources_cmd(padapter); + if (_FAIL == rtw_pwr_wakeup(padapter)) + DBG_871X("%s(): rtw_pwr_wakeup fail !!!\n", __func__); + } + + spin_unlock_bh(&pmlmepriv->lock); + + return true; +} + +u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 res = true; + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("+rtw_set_802_11_bssid_list_scan(), fw_state =%x\n", get_fwstate(pmlmepriv))); + + if (padapter == NULL) { + res = false; + goto exit; + } + if (padapter->hw_init_completed == false) { + res = false; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n ===rtw_set_802_11_bssid_list_scan:hw_init_completed ==false ===\n")); + goto exit; + } + + if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) || + (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_bssid_list_scan fail since fw_state = %x\n", get_fwstate(pmlmepriv))); + res = true; + + if (check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING)) == true) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n\n")); + } else { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###pmlmepriv->sitesurveyctrl.traffic_busy ==true\n\n")); + } + } else { + if (rtw_is_scan_deny(padapter)) { + DBG_871X(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter)); + indicate_wx_scan_complete_event(padapter); + return _SUCCESS; + } + + spin_lock_bh(&pmlmepriv->lock); + + res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, NULL, 0); + + spin_unlock_bh(&pmlmepriv->lock); + } +exit: + + return res; +} + +u8 rtw_set_802_11_authentication_mode(struct adapter *padapter, enum NDIS_802_11_AUTHENTICATION_MODE authmode) +{ + struct security_priv *psecuritypriv = &padapter->securitypriv; + int res; + u8 ret; + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_802_11_auth.mode(): mode =%x\n", authmode)); + + psecuritypriv->ndisauthtype = authmode; + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_authentication_mode:psecuritypriv->ndisauthtype =%d", psecuritypriv->ndisauthtype)); + + if (psecuritypriv->ndisauthtype > 3) + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + + res = rtw_set_auth(padapter, psecuritypriv); + + if (res == _SUCCESS) + ret = true; + else + ret = false; + + return ret; +} + +u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct ndis_802_11_wep *wep) +{ + + u8 bdefaultkey; + u8 btransmitkey; + sint keyid, res; + struct security_priv *psecuritypriv = &(padapter->securitypriv); + u8 ret = _SUCCESS; + + bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true; /* for ??? */ + btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true : false; /* for ??? */ + keyid = wep->KeyIndex & 0x3fffffff; + + if (keyid >= 4) { + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("MgntActrtw_set_802_11_add_wep:keyid>4 =>fail\n")); + ret = false; + goto exit; + } + + switch (wep->KeyLength) { + case 5: + psecuritypriv->dot11PrivacyAlgrthm = _WEP40_; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength =5\n")); + break; + case 13: + psecuritypriv->dot11PrivacyAlgrthm = _WEP104_; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength = 13\n")); + break; + default: + psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_; + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength!=5 or 13\n")); + break; + } + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, + ("rtw_set_802_11_add_wep:before memcpy, wep->KeyLength = 0x%x wep->KeyIndex = 0x%x keyid =%x\n", + wep->KeyLength, wep->KeyIndex, keyid)); + + memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength); + + psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength; + + psecuritypriv->dot11PrivacyKeyIndex = keyid; + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_add_wep:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x\n", + psecuritypriv->dot11DefKey[keyid].skey[0], psecuritypriv->dot11DefKey[keyid].skey[1], psecuritypriv->dot11DefKey[keyid].skey[2], + psecuritypriv->dot11DefKey[keyid].skey[3], psecuritypriv->dot11DefKey[keyid].skey[4], psecuritypriv->dot11DefKey[keyid].skey[5], + psecuritypriv->dot11DefKey[keyid].skey[6], psecuritypriv->dot11DefKey[keyid].skey[7], psecuritypriv->dot11DefKey[keyid].skey[8], + psecuritypriv->dot11DefKey[keyid].skey[9], psecuritypriv->dot11DefKey[keyid].skey[10], psecuritypriv->dot11DefKey[keyid].skey[11], + psecuritypriv->dot11DefKey[keyid].skey[12])); + + res = rtw_set_key(padapter, psecuritypriv, keyid, 1, true); + + if (res == _FAIL) + ret = false; +exit: + + return ret; +} + +/* +* rtw_get_cur_max_rate - +* @adapter: pointer to struct adapter structure +* +* Return 0 or 100Kbps +*/ +u16 rtw_get_cur_max_rate(struct adapter *adapter) +{ + int i = 0; + u16 rate = 0, max_rate = 0; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_bssid_ex *pcur_bss = &pmlmepriv->cur_network.network; + struct sta_info *psta = NULL; + u8 short_GI = 0; + u8 rf_type = 0; + + if ((check_fwstate(pmlmepriv, _FW_LINKED) != true) + && (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) != true)) + return 0; + + psta = rtw_get_stainfo(&adapter->stapriv, get_bssid(pmlmepriv)); + if (psta == NULL) + return 0; + + short_GI = query_ra_short_GI(psta); + + if (IsSupportedHT(psta->wireless_mode)) { + rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + + max_rate = rtw_mcs_rate( + rf_type, + ((psta->bw_mode == CHANNEL_WIDTH_40)?1:0), + short_GI, + psta->htpriv.ht_cap.supp_mcs_set + ); + } 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/rtl8723bs/core/rtw_mlme.c b/drivers/staging/rtl8723bs/core/rtw_mlme.c new file mode 100644 index 0000000..9e35573 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c @@ -0,0 +1,3150 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <linux/jiffies.h> + +extern u8 rtw_do_join(struct adapter *padapter); + +sint _rtw_init_mlme_priv(struct adapter *padapter) +{ + sint i; + u8 *pbuf; + struct wlan_network *pnetwork; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + sint res = _SUCCESS; + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */ + /* memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv)); */ + + pmlmepriv->nic_hdl = (u8 *)padapter; + + pmlmepriv->pscanned = NULL; + pmlmepriv->fw_state = WIFI_STATION_STATE; /* Must sync with rtw_wdev_alloc() */ + /* wdev->iftype = NL80211_IFTYPE_STATION */ + pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown; + pmlmepriv->scan_mode = SCAN_ACTIVE;/* 1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */ + + spin_lock_init(&(pmlmepriv->lock)); + _rtw_init_queue(&(pmlmepriv->free_bss_pool)); + _rtw_init_queue(&(pmlmepriv->scanned_queue)); + + set_scanned_network_val(pmlmepriv, 0); + + memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid)); + + pbuf = vzalloc(MAX_BSS_CNT * (sizeof(struct wlan_network))); + + if (pbuf == NULL) { + res = _FAIL; + goto exit; + } + pmlmepriv->free_bss_buf = pbuf; + + pnetwork = (struct wlan_network *)pbuf; + + for (i = 0; i < MAX_BSS_CNT; i++) { + INIT_LIST_HEAD(&(pnetwork->list)); + + list_add_tail(&(pnetwork->list), &(pmlmepriv->free_bss_pool.queue)); + + pnetwork++; + } + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + + rtw_clear_scan_deny(padapter); + + #define RTW_ROAM_SCAN_RESULT_EXP_MS 5000 + #define RTW_ROAM_RSSI_DIFF_TH 10 + #define RTW_ROAM_SCAN_INTERVAL_MS 10000 + + pmlmepriv->roam_flags = 0 + | RTW_ROAM_ON_EXPIRED + | RTW_ROAM_ON_RESUME + #ifdef CONFIG_LAYER2_ROAMING_ACTIVE /* FIXME */ + | RTW_ROAM_ACTIVE + #endif + ; + + pmlmepriv->roam_scanr_exp_ms = RTW_ROAM_SCAN_RESULT_EXP_MS; + pmlmepriv->roam_rssi_diff_th = RTW_ROAM_RSSI_DIFF_TH; + pmlmepriv->roam_scan_int_ms = RTW_ROAM_SCAN_INTERVAL_MS; + + rtw_init_mlme_timer(padapter); + +exit: + + return res; +} + +static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen) +{ + if (*ppie) { + kfree(*ppie); + *plen = 0; + *ppie = NULL; + } +} + +void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv) +{ + rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len); + rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len); + rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len); + + rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len); + rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len); +} + +void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv) +{ + rtw_free_mlme_priv_ie_data(pmlmepriv); + + if (pmlmepriv) { + if (pmlmepriv->free_bss_buf) { + vfree(pmlmepriv->free_bss_buf); + } + } +} + +/* +struct wlan_network *_rtw_dequeue_network(struct __queue *queue) +{ + _irqL irqL; + + struct wlan_network *pnetwork; + + spin_lock_bh(&queue->lock); + + if (list_empty(&queue->queue)) + + pnetwork = NULL; + + else + { + pnetwork = LIST_CONTAINOR(get_next(&queue->queue), struct wlan_network, list); + + list_del_init(&(pnetwork->list)); + } + + spin_unlock_bh(&queue->lock); + + return pnetwork; +} +*/ + +struct wlan_network *_rtw_alloc_network(struct mlme_priv *pmlmepriv)/* _queue *free_queue) */ +{ + struct wlan_network *pnetwork; + struct __queue *free_queue = &pmlmepriv->free_bss_pool; + struct list_head *plist = NULL; + + spin_lock_bh(&free_queue->lock); + + if (list_empty(&free_queue->queue)) { + pnetwork = NULL; + goto exit; + } + plist = get_next(&(free_queue->queue)); + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + list_del_init(&pnetwork->list); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("_rtw_alloc_network: ptr =%p\n", plist)); + pnetwork->network_type = 0; + pnetwork->fixed = false; + pnetwork->last_scanned = jiffies; + pnetwork->aid = 0; + pnetwork->join_res = 0; + + pmlmepriv->num_of_scanned++; + +exit: + spin_unlock_bh(&free_queue->lock); + + return pnetwork; +} + +void _rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall) +{ + unsigned int delta_time; + u32 lifetime = SCANQUEUE_LIFETIME; +/* _irqL irqL; */ + struct __queue *free_queue = &(pmlmepriv->free_bss_pool); + + if (pnetwork == NULL) + return; + + if (pnetwork->fixed == true) + return; + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) + lifetime = 1; + + if (!isfreeall) { + delta_time = jiffies_to_msecs(jiffies - pnetwork->last_scanned); + if (delta_time < lifetime)/* unit:msec */ + return; + } + + spin_lock_bh(&free_queue->lock); + + list_del_init(&(pnetwork->list)); + + list_add_tail(&(pnetwork->list), &(free_queue->queue)); + + pmlmepriv->num_of_scanned--; + + + /* DBG_871X("_rtw_free_network:SSID =%s\n", pnetwork->network.Ssid.Ssid); */ + + spin_unlock_bh(&free_queue->lock); +} + +void _rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork) +{ + + struct __queue *free_queue = &(pmlmepriv->free_bss_pool); + + if (pnetwork == NULL) + return; + + if (pnetwork->fixed == true) + return; + + /* spin_lock_irqsave(&free_queue->lock, irqL); */ + + list_del_init(&(pnetwork->list)); + + list_add_tail(&(pnetwork->list), get_list_head(free_queue)); + + pmlmepriv->num_of_scanned--; + + /* spin_unlock_irqrestore(&free_queue->lock, irqL); */ +} + +/* + return the wlan_network with the matching addr + + Shall be calle under atomic context... to avoid possible racing condition... +*/ +struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr) +{ + struct list_head *phead, *plist; + struct wlan_network *pnetwork = NULL; + u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + + if (!memcmp(zero_addr, addr, ETH_ALEN)) { + pnetwork = NULL; + goto exit; + } + + /* spin_lock_bh(&scanned_queue->lock); */ + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (plist != phead) { + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN)) + break; + + plist = get_next(plist); + } + + if (plist == phead) + pnetwork = NULL; + + /* spin_unlock_bh(&scanned_queue->lock); */ + +exit: + return pnetwork; +} + +void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall) +{ + struct list_head *phead, *plist; + struct wlan_network *pnetwork; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct __queue *scanned_queue = &pmlmepriv->scanned_queue; + + spin_lock_bh(&scanned_queue->lock); + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (phead != plist) { + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + plist = get_next(plist); + + _rtw_free_network(pmlmepriv, pnetwork, isfreeall); + + } + + spin_unlock_bh(&scanned_queue->lock); +} + + + + +sint rtw_if_up(struct adapter *padapter) +{ + + sint res; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved || + (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved)); + res = false; + } else + res = true; + return res; +} + + +void rtw_generate_random_ibss(u8 *pibss) +{ + 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_capability_from_ie(u8 *ie) +{ + return (ie + 8 + 2); +} + + +u16 rtw_get_capability(struct wlan_bssid_ex *bss) +{ + __le16 val; + + memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); +} + +u8 *rtw_get_beacon_interval_from_ie(u8 *ie) +{ + return (ie + 8); +} + + +int rtw_init_mlme_priv(struct adapter *padapter)/* struct mlme_priv *pmlmepriv) */ +{ + int res; + + res = _rtw_init_mlme_priv(padapter);/* (pmlmepriv); */ + return res; +} + +void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv) +{ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv\n")); + _rtw_free_mlme_priv(pmlmepriv); +} + +/* +static struct wlan_network *rtw_dequeue_network(struct __queue *queue) +{ + struct wlan_network *pnetwork; + + pnetwork = _rtw_dequeue_network(queue); + return pnetwork; +} +*/ + +struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv); +struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)/* _queue *free_queue) */ +{ + struct wlan_network *pnetwork; + + pnetwork = _rtw_alloc_network(pmlmepriv); + return pnetwork; +} + +void rtw_free_network_nolock(struct adapter *padapter, struct wlan_network *pnetwork); +void rtw_free_network_nolock(struct adapter *padapter, 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_network_nolock(&(padapter->mlmepriv), pnetwork); + rtw_cfg80211_unlink_bss(padapter, pnetwork); +} + + +void rtw_free_network_queue(struct adapter *dev, u8 isfreeall) +{ + _rtw_free_network_queue(dev, isfreeall); +} + +/* + return the wlan_network with the matching addr + + Shall be calle under atomic context... to avoid possible racing condition... +*/ +struct wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr) +{ + struct wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr); + + return pnetwork; +} + +int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork) +{ + int ret = true; + struct security_priv *psecuritypriv = &adapter->securitypriv; + + if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) && + (pnetwork->network.Privacy == 0)) + ret = false; + else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) && + (pnetwork->network.Privacy == 1)) + ret = false; + else + ret = true; + + return ret; + +} + +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.SsidLength == b->Ssid.SsidLength) + && !memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength); +} + +int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst, u8 feature) +{ + u16 s_cap, d_cap; + __le16 tmps, tmpd; + + if (rtw_bug_check(dst, src, &s_cap, &d_cap) == false) + return false; + + memcpy((u8 *)&tmps, rtw_get_capability_from_ie(src->IEs), 2); + memcpy((u8 *)&tmpd, rtw_get_capability_from_ie(dst->IEs), 2); + + + s_cap = le16_to_cpu(tmps); + d_cap = le16_to_cpu(tmpd); + + return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) && + /* (src->Configuration.DSConfig == dst->Configuration.DSConfig) && */ + ((!memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN))) && + ((!memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength))) && + ((s_cap & WLAN_CAPABILITY_IBSS) == + (d_cap & WLAN_CAPABILITY_IBSS)) && + ((s_cap & WLAN_CAPABILITY_BSS) == + (d_cap & WLAN_CAPABILITY_BSS))); + +} + +struct wlan_network *_rtw_find_same_network(struct __queue *scanned_queue, struct wlan_network *network) +{ + struct list_head *phead, *plist; + struct wlan_network *found = NULL; + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (plist != phead) { + found = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (is_same_network(&network->network, &found->network, 0)) + break; + + plist = get_next(plist); + } + + if (plist == phead) + found = NULL; + + return found; +} + +struct wlan_network *rtw_get_oldest_wlan_network(struct __queue *scanned_queue) +{ + struct list_head *plist, *phead; + + + struct wlan_network *pwlan = NULL; + struct wlan_network *oldest = NULL; + + phead = get_list_head(scanned_queue); + + plist = get_next(phead); + + while (1) { + + if (phead == plist) + break; + + pwlan = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (pwlan->fixed != true) { + if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned)) + oldest = pwlan; + } + + plist = get_next(plist); + } + return oldest; + +} + +void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src, + struct adapter *padapter, bool update_ie) +{ + long rssi_ori = dst->Rssi; + + u8 sq_smp = src->PhyInfo.SignalQuality; + + u8 ss_final; + u8 sq_final; + long rssi_final; + + #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1 + if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + DBG_871X(FUNC_ADPT_FMT" %s("MAC_FMT", ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n" + , FUNC_ADPT_ARG(padapter) + , src->Ssid.Ssid, MAC_ARG(src->MacAddress), src->Configuration.DSConfig + , ss_ori, sq_ori, rssi_ori + , ss_smp, sq_smp, rssi_smp + ); + } + #endif + + /* The rule below is 1/5 for sample value, 4/5 for history value */ + if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src, 0)) { + /* 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) { + dst->Reserved[0] = src->Reserved[0]; + dst->Reserved[1] = src->Reserved[1]; + memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src)); + } + + dst->PhyInfo.SignalStrength = ss_final; + dst->PhyInfo.SignalQuality = sq_final; + dst->Rssi = rssi_final; + + #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1 + if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + DBG_871X(FUNC_ADPT_FMT" %s("MAC_FMT"), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n" + , FUNC_ADPT_ARG(padapter) + , dst->Ssid.Ssid, MAC_ARG(dst->MacAddress), dst->PhyInfo.SignalStrength, dst->PhyInfo.SignalQuality, dst->Rssi); + } + #endif +} + +static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + rtw_bug_check(&(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network)); + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork, 0))) { + /* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"Same Network\n"); */ + + /* if (pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */ + { + update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true); + rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(struct ndis_802_11_fix_ie), + pmlmepriv->cur_network.network.IELength); + } + } +} + + +/* + +Caller must hold pmlmepriv->lock first. + + +*/ +void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target) +{ + struct list_head *plist, *phead; + u32 bssid_ex_sz; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct __queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *oldest = NULL; + int target_find = 0; + u8 feature = 0; + + spin_lock_bh(&queue->lock); + phead = get_list_head(queue); + plist = get_next(phead); + + while (1) { + if (phead == plist) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + rtw_bug_check(pnetwork, pnetwork, pnetwork, pnetwork); + + if (is_same_network(&(pnetwork->network), target, feature)) { + target_find = 1; + break; + } + + if (rtw_roam_flags(adapter)) { + /* TODO: don't select netowrk in the same ess as oldest if it's new enough*/ + } + + if (oldest == NULL || time_after(oldest->last_scanned, pnetwork->last_scanned)) + oldest = pnetwork; + + plist = get_next(plist); + + } + + + /* If we didn't find a match, then get a new network slot to initialize + * with this beacon's information */ + /* if (phead == plist) { */ + if (!target_find) { + if (list_empty(&pmlmepriv->free_bss_pool.queue)) { + /* If there are no more slots, expire the oldest */ + /* list_del_init(&oldest->list); */ + pnetwork = oldest; + if (pnetwork == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n")); + goto exit; + } + memcpy(&(pnetwork->network), target, get_wlan_bssid_ex_sz(target)); + /* variable initialize */ + pnetwork->fixed = false; + pnetwork->last_scanned = jiffies; + + pnetwork->network_type = 0; + pnetwork->aid = 0; + pnetwork->join_res = 0; + + /* bss info not receving from the right channel */ + if (pnetwork->network.PhyInfo.SignalQuality == 101) + pnetwork->network.PhyInfo.SignalQuality = 0; + } else { + /* Otherwise just pull from the free list */ + + pnetwork = rtw_alloc_network(pmlmepriv); /* will update scan_time */ + + if (pnetwork == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n")); + goto exit; + } + + bssid_ex_sz = get_wlan_bssid_ex_sz(target); + target->Length = bssid_ex_sz; + memcpy(&(pnetwork->network), target, bssid_ex_sz); + + pnetwork->last_scanned = jiffies; + + /* bss info not receving from the right channel */ + if (pnetwork->network.PhyInfo.SignalQuality == 101) + pnetwork->network.PhyInfo.SignalQuality = 0; + + list_add_tail(&(pnetwork->list), &(queue->queue)); + + } + } else { + /* we have an entry and we are going to update it. But this entry may + * be already expired. In this case we do the same as we found a new + * net and call the new_net handler + */ + bool update_ie = true; + + pnetwork->last_scanned = jiffies; + + /* target.Reserved[0]== 1, means that scaned network is a bcn frame. */ + if ((pnetwork->network.IELength > target->IELength) && (target->Reserved[0] == 1)) + update_ie = false; + + /* probe resp(3) > beacon(1) > probe req(2) */ + if ((target->Reserved[0] != 2) && + (target->Reserved[0] >= pnetwork->network.Reserved[0]) + ) { + update_ie = true; + } else { + update_ie = false; + } + + update_network(&(pnetwork->network), target, adapter, update_ie); + } + +exit: + spin_unlock_bh(&queue->lock); +} + +void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork); +void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork) +{ + /* struct __queue *queue = &(pmlmepriv->scanned_queue); */ + + /* spin_lock_bh(&queue->lock); */ + + update_current_network(adapter, pnetwork); + + rtw_update_scanned_network(adapter, pnetwork); + + /* spin_unlock_bh(&queue->lock); */ +} + +/* 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 adapter *adapter, struct wlan_network *pnetwork); +int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork) +{ + struct security_priv *psecuritypriv = &adapter->securitypriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u32 desired_encmode; + u32 privacy; + + /* u8 wps_ie[512]; */ + uint wps_ielen; + + int bselected = true; + + desired_encmode = psecuritypriv->ndisencryptstatus; + privacy = pnetwork->network.Privacy; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + if (rtw_get_wps_ie(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL) + return true; + else + return false; + + } + if (adapter->registrypriv.wifi_spec == 1) { /* for correct flow of 8021X to do.... */ + u8 *p = NULL; + uint ie_len = 0; + + if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0)) + bselected = false; + + if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) { + p = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pnetwork->network.IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + bselected = true; + } else { + bselected = false; + } + } + } + + + if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) { + DBG_871X("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy); + bselected = false; + } + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) { + if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode) + bselected = false; + } + + + return bselected; +} + +/* TODO: Perry : For Power Management */ +void rtw_atimdone_event_callback(struct adapter *adapter, u8 *pbuf) +{ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("receive atimdone_evet\n")); +} + + +void rtw_survey_event_callback(struct adapter *adapter, u8 *pbuf) +{ + u32 len; + struct wlan_bssid_ex *pnetwork; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + pnetwork = (struct wlan_bssid_ex *)pbuf; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_survey_event_callback, ssid =%s\n", pnetwork->Ssid.Ssid)); + + len = get_wlan_bssid_ex_sz(pnetwork); + if (len > (sizeof(struct wlan_bssid_ex))) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n ****rtw_survey_event_callback: return a wrong bss ***\n")); + return; + } + + + spin_lock_bh(&pmlmepriv->lock); + + /* update IBSS_network 's timestamp */ + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) { + /* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_callback : WIFI_ADHOC_MASTER_STATE\n\n"); */ + if (!memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN)) { + struct wlan_network *ibss_wlan = NULL; + + memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8); + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->MacAddress); + if (ibss_wlan) { + memcpy(ibss_wlan->network.IEs, pnetwork->IEs, 8); + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + goto exit; + } + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + } + } + + /* lock pmlmepriv->lock when you accessing network_q */ + if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == false) { + if (pnetwork->Ssid.Ssid[0] == 0) { + pnetwork->Ssid.SsidLength = 0; + } + rtw_add_network(adapter, pnetwork); + } + +exit: + + spin_unlock_bh(&pmlmepriv->lock); + + return; +} + + + +void rtw_surveydone_event_callback(struct adapter *adapter, u8 *pbuf) +{ + u8 timer_cancelled = false; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + spin_lock_bh(&pmlmepriv->lock); + if (pmlmepriv->wps_probe_req_ie) { + pmlmepriv->wps_probe_req_ie_len = 0; + kfree(pmlmepriv->wps_probe_req_ie); + pmlmepriv->wps_probe_req_ie = NULL; + } + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback: fw_state:%x\n\n", get_fwstate(pmlmepriv))); + + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) { + /* u8 timer_cancelled; */ + + timer_cancelled = true; + /* _cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled); */ + + _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY); + } else { + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv))); + } + spin_unlock_bh(&pmlmepriv->lock); + + if (timer_cancelled) + _cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled); + + + spin_lock_bh(&pmlmepriv->lock); + + rtw_set_signal_stat_timer(&adapter->recvpriv); + + if (pmlmepriv->to_join == 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_queue(pmlmepriv) == _SUCCESS) { + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + } else{ + struct wlan_bssid_ex *pdev_network = &(adapter->registrypriv.dev_network); + u8 *pibss = adapter->registrypriv.dev_network.MacAddress; + + /* pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;because don't set assoc_timer */ + _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n")); + + memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid)); + memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid)); + + rtw_update_registrypriv_dev_network(adapter); + rtw_generate_random_ibss(pibss); + + pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE; + + if (rtw_createbss_cmd(adapter) != _SUCCESS) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd status FAIL\n")); + } + + pmlmepriv->to_join = false; + } + } + } else{ + int s_ret; + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + pmlmepriv->to_join = false; + s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv); + if (_SUCCESS == s_ret) { + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + } else if (s_ret == 2) {/* there is no need to wait for join */ + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + rtw_indicate_connect(adapter); + } else{ + DBG_871X("try_to_join, but select scanning queue fail, to_roam:%d\n", rtw_to_roam(adapter)); + + if (rtw_to_roam(adapter) != 0) { + if (rtw_dec_to_roam(adapter) == 0 + || _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0) + ) { + rtw_set_to_roam(adapter, 0); +#ifdef CONFIG_INTEL_WIDI + if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) { + memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN); + intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0); + DBG_871X("change to widi listen\n"); + } +#endif /* CONFIG_INTEL_WIDI */ + rtw_free_assoc_resources(adapter, 1); + rtw_indicate_disconnect(adapter); + } else { + pmlmepriv->to_join = true; + } + } else + rtw_indicate_disconnect(adapter); + + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + } + } + } else { + if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) { + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) + && check_fwstate(pmlmepriv, _FW_LINKED)) { + if (rtw_select_roaming_candidate(pmlmepriv) == _SUCCESS) { + receive_disconnect(adapter, pmlmepriv->cur_network.network.MacAddress + , WLAN_REASON_ACTIVE_ROAM); + } + } + } + } + + /* DBG_871X("scan complete in %dms\n", jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time)); */ + + spin_unlock_bh(&pmlmepriv->lock); + + rtw_os_xmit_schedule(adapter); + + rtw_cfg80211_surveydone_event_callback(adapter); + + rtw_indicate_scan_done(adapter, false); +} + +void rtw_dummy_event_callback(struct adapter *adapter, u8 *pbuf) +{ +} + +void rtw_fwdbg_event_callback(struct adapter *adapter, u8 *pbuf) +{ +} + +static void free_scanqueue(struct mlme_priv *pmlmepriv) +{ + struct __queue *free_queue = &pmlmepriv->free_bss_pool; + struct __queue *scan_queue = &pmlmepriv->scanned_queue; + struct list_head *plist, *phead, *ptemp; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n")); + spin_lock_bh(&scan_queue->lock); + spin_lock_bh(&free_queue->lock); + + phead = get_list_head(scan_queue); + plist = get_next(phead); + + while (plist != phead) { + ptemp = get_next(plist); + list_del_init(plist); + list_add_tail(plist, &free_queue->queue); + plist = ptemp; + pmlmepriv->num_of_scanned--; + } + + spin_unlock_bh(&free_queue->lock); + spin_unlock_bh(&scan_queue->lock); +} + +static void rtw_reset_rx_info(struct debug_priv *pdbgpriv) +{ + pdbgpriv->dbg_rx_ampdu_drop_count = 0; + pdbgpriv->dbg_rx_ampdu_forced_indicate_count = 0; + pdbgpriv->dbg_rx_ampdu_loss_count = 0; + pdbgpriv->dbg_rx_dup_mgt_frame_drop_count = 0; + pdbgpriv->dbg_rx_ampdu_window_shift_cnt = 0; +} + +static void find_network(struct adapter *adapter) +{ + struct wlan_network *pwlan = NULL; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + + pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress); + if (pwlan) + pwlan->fixed = false; + else + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_assoc_resources : pwlan == NULL\n\n")); + + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && + (adapter->stapriv.asoc_sta_count == 1)) + rtw_free_network_nolock(adapter, pwlan); +} + +/* +*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock +*/ +void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + struct sta_priv *pstapriv = &adapter->stapriv; + struct dvobj_priv *psdpriv = adapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources\n")); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress ="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_stainfo(&adapter->stapriv, tgt_network->network.MacAddress); + spin_lock_bh(&(pstapriv->sta_hash_lock)); + rtw_free_stainfo(adapter, psta); + + spin_unlock_bh(&(pstapriv->sta_hash_lock)); + + } + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)) { + struct sta_info *psta; + + rtw_free_all_stainfo(adapter); + + psta = rtw_get_bcmc_stainfo(adapter); + rtw_free_stainfo(adapter, psta); + + rtw_init_bcmc_stainfo(adapter); + } + + find_network(adapter); + + if (lock_scanned_queue) + adapter->securitypriv.key_mask = 0; + + rtw_reset_rx_info(pdbgpriv); +} + +/* +*rtw_indicate_connect: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_connect(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect\n")); + + pmlmepriv->to_join = false; + + if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) { + + set_fwstate(pmlmepriv, _FW_LINKED); + + rtw_os_indicate_connect(padapter); + } + + rtw_set_to_roam(padapter, 0); +#ifdef CONFIG_INTEL_WIDI + if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) { + memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN); + intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_WK, NULL, 0); + DBG_871X("change to widi listen\n"); + } +#endif /* CONFIG_INTEL_WIDI */ + + rtw_set_scan_deny(padapter, 3000); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect: fw_state = 0x%08x\n", get_fwstate(pmlmepriv))); +} + +/* +*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_disconnect(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n")); + + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS); + + /* DBG_871X("clear wps when %s\n", __func__); */ + + if (rtw_to_roam(padapter) > 0) + _clr_fwstate_(pmlmepriv, _FW_LINKED); + + if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) + || (rtw_to_roam(padapter) <= 0) + ) { + rtw_os_indicate_disconnect(padapter); + + /* set ips_deny_time to avoid enter IPS before LPS leave */ + rtw_set_ips_deny(padapter, 3000); + + _clr_fwstate_(pmlmepriv, _FW_LINKED); + + rtw_clear_scan_deny(padapter); + } + + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1); +} + +inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted) +{ + DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + rtw_os_indicate_scan_done(padapter, aborted); + + if (is_primary_adapter(padapter) && + (!adapter_to_pwrctl(padapter)->bInSuspend) && + (!check_fwstate(&padapter->mlmepriv, + WIFI_ASOC_STATE|WIFI_UNDER_LINKING))) { + struct pwrctrl_priv *pwrpriv; + + pwrpriv = adapter_to_pwrctl(padapter); + rtw_set_ips_deny(padapter, 0); + _set_timer(&padapter->mlmepriv.dynamic_chk_timer, 1); + } +} + +void rtw_scan_abort(struct 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(start) <= 200) { + + if (adapter->bDriverStopped || adapter->bSurpriseRemoved) + break; + + DBG_871X(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_871X(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev)); + rtw_indicate_scan_done(adapter, true); + } + pmlmeext->scan_abort = false; +} + +static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, struct wlan_network *pnetwork) +{ + int i; + struct sta_info *bmc_sta, *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress); + if (psta == NULL) { + psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress); + } + + if (psta) { /* update ptarget_sta */ + + DBG_871X("%s\n", __func__); + + psta->aid = pnetwork->join_res; + + update_sta_info(padapter, psta); + + /* update station supportRate */ + psta->bssratelen = rtw_get_rateset_len(pnetwork->network.SupportedRates); + memcpy(psta->bssrateset, pnetwork->network.SupportedRates, psta->bssratelen); + rtw_hal_update_sta_rate_mask(padapter, psta); + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + psta->raid = rtw_hal_networktype_to_raid(padapter, psta); + + + /* sta mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true); + + /* security related */ + if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + padapter->securitypriv.binstallGrpkey = false; + padapter->securitypriv.busetkipkey = false; + padapter->securitypriv.bgrpkey_handshake = false; + + psta->ieee8021x_blocked = true; + psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm; + + memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype)); + + memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype)); + memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype)); + + memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48)); + psta->dot11txpn.val = psta->dot11txpn.val + 1; + memset((u8 *)&psta->dot11wtxpn, 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; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__, + preorder_ctrl->indicate_seq); + #endif + preorder_ctrl->wend_b = 0xffff; + preorder_ctrl->wsize_b = 64;/* max_ampdu_sz;ex. 32(kbytes) -> wsize_b =32 */ + } + + + bmc_sta = rtw_get_bcmc_stainfo(padapter); + if (bmc_sta) { + for (i = 0; i < 16 ; i++) { + /* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */ + preorder_ctrl = &bmc_sta->recvreorder_ctrl[i]; + preorder_ctrl->enable = false; + preorder_ctrl->indicate_seq = 0xffff; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__, + preorder_ctrl->indicate_seq); + #endif + preorder_ctrl->wend_b = 0xffff; + preorder_ctrl->wsize_b = 64;/* max_ampdu_sz;ex. 32(kbytes) -> wsize_b =32 */ + } + } + } + + return psta; + +} + +/* pnetwork : returns from rtw_joinbss_event_callback */ +/* ptarget_wlan: found from scanned_queue */ +static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + + DBG_871X("%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); + #if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1 + DBG_871X(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u" + "\n" + , FUNC_ADPT_ARG(padapter) + , padapter->recvpriv.signal_strength + , padapter->recvpriv.rssi + , padapter->recvpriv.signal_qual + ); + #endif + + rtw_set_signal_stat_timer(&padapter->recvpriv); + + /* update fw_state will clr _FW_UNDER_LINKING here indirectly */ + switch (pnetwork->network.InfrastructureMode) { + case Ndis802_11Infrastructure: + + if (pmlmepriv->fw_state&WIFI_UNDER_WPS) + pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS; + else + pmlmepriv->fw_state = WIFI_STATION_STATE; + + break; + case Ndis802_11IBSS: + pmlmepriv->fw_state = WIFI_ADHOC_STATE; + break; + default: + pmlmepriv->fw_state = WIFI_NULL_STATE; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n")); + break; + } + + rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof(struct ndis_802_11_fix_ie), + (cur_network->network.IELength)); + + rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength, (u8) cur_network->network.Configuration.DSConfig); +} + +/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) */ +/* pnetwork : returns from rtw_joinbss_event_callback */ +/* ptarget_wlan: found from scanned_queue */ +/* if join_res > 0, for (fw_state ==WIFI_STATION_STATE), we check if "ptarget_sta" & "ptarget_wlan" exist. */ +/* if join_res > 0, for (fw_state ==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */ +/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan != NULL). */ +/* */ +/* define REJOIN */ +void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf) +{ + static u8 retry = 0; + u8 timer_cancelled; + struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct wlan_network *pnetwork = (struct wlan_network *)pbuf; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct wlan_network *pcur_wlan = NULL, *ptarget_wlan = NULL; + unsigned int the_same_macaddr = false; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("joinbss event call back received with res =%d\n", pnetwork->join_res)); + + rtw_get_encrypt_decrypt_from_registrypriv(adapter); + + + if (pmlmepriv->assoc_ssid.SsidLength == 0) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@ joinbss event call back for Any SSid\n")); + } else{ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@ rtw_joinbss_event_callback for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid)); + } + + the_same_macaddr = !memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN); + + pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network); + if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n")); + return; + } + + spin_lock_bh(&pmlmepriv->lock); + + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_joinbss_event_callback !! spin_lock_irqsave\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_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + } else{ + pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + if (pcur_wlan) + pcur_wlan->fixed = false; + + pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress); + if (pcur_sta) + rtw_free_stainfo(adapter, pcur_sta); + + ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress); + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + if (ptarget_wlan) + ptarget_wlan->fixed = true; + } + } + + } else{ + ptarget_wlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, pnetwork); + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + if (ptarget_wlan) + ptarget_wlan->fixed = true; + } + } + + /* s2. update cur_network */ + if (ptarget_wlan) { + rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork); + } else{ + DBG_871X_LEVEL(_drv_always_, "Can't find ptarget_wlan when joinbss_event callback\n"); + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + goto ignore_joinbss_callback; + } + + + /* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork); + if (ptarget_sta == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't update stainfo when joinbss_event callback\n")); + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + goto ignore_joinbss_callback; + } + } + + /* s4. indicate connect */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + pmlmepriv->cur_network_scanned = ptarget_wlan; + rtw_indicate_connect(adapter); + } else{ + /* adhoc mode will rtw_indicate_connect when rtw_stassoc_event_callback */ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv))); + } + + + /* s5. Cancle assoc_timer */ + _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n")); + + } else{ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv))); + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + goto ignore_joinbss_callback; + } + + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + + } else if (pnetwork->join_res == -4) { + rtw_reset_securitypriv(adapter); + _set_timer(&pmlmepriv->assoc_timer, 1); + + /* rtw_free_assoc_resources(adapter, 1); */ + + if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("fail! clear _FW_UNDER_LINKING ^^^fw_state =%x\n", get_fwstate(pmlmepriv))); + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + } + + } else{/* if join_res < 0 (join fails), then try again */ + + #ifdef REJOIN + res = _FAIL; + if (retry < 2) { + res = rtw_select_and_join_from_scanned_queue(pmlmepriv); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_select_and_join_from_scanned_queue again! res:%d\n", res)); + } + + if (res == _SUCCESS) { + /* extend time of assoc_timer */ + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + retry++; + } else if (res == 2) {/* there is no need to wait for join */ + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + rtw_indicate_connect(adapter); + } else{ + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Set Assoc_Timer = 1; can't find match ssid in scanned_q\n")); + #endif + + _set_timer(&pmlmepriv->assoc_timer, 1); + /* rtw_free_assoc_resources(adapter, 1); */ + _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING); + + #ifdef REJOIN + retry = 0; + } + #endif + } + +ignore_joinbss_callback: + + spin_unlock_bh(&pmlmepriv->lock); +} + +void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf) +{ + struct wlan_network *pnetwork = (struct wlan_network *)pbuf; + + mlmeext_joinbss_event_callback(adapter, pnetwork->join_res); + + rtw_os_xmit_schedule(adapter); +} + +/* FOR STA, AP , AD-HOC mode */ +void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta, u32 mstatus) +{ + u16 media_status_rpt; + + if (psta == NULL) + return; + + media_status_rpt = (u16)((psta->mac_id<<8)|mstatus); /* MACID|OPMODE:1 connect */ + rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status_rpt); +} + +void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf) +{ + struct sta_info *psta; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct stassoc_event *pstassoc = (struct stassoc_event *)pbuf; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct wlan_network *ptarget_wlan = NULL; + + if (rtw_access_ctrl(adapter, pstassoc->macaddr) == false) + return; + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr); + if (psta) { + u8 *passoc_req = NULL; + u32 assoc_req_len = 0; + + rtw_sta_media_status_rpt(adapter, psta, 1); + +#ifndef CONFIG_AUTO_AP_MODE + + ap_sta_info_defer_update(adapter, psta); + + /* report to upper layer */ + DBG_871X("indicate_sta_assoc_event to upper layer - hostapd\n"); + spin_lock_bh(&psta->lock); + if (psta->passoc_req && psta->assoc_req_len > 0) { + passoc_req = rtw_zmalloc(psta->assoc_req_len); + if (passoc_req) { + assoc_req_len = psta->assoc_req_len; + memcpy(passoc_req, psta->passoc_req, assoc_req_len); + + kfree(psta->passoc_req); + psta->passoc_req = NULL; + psta->assoc_req_len = 0; + } + } + spin_unlock_bh(&psta->lock); + + if (passoc_req && assoc_req_len > 0) { + rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len); + + kfree(passoc_req); + } +#endif /* CONFIG_AUTO_AP_MODE */ + } + return; + } + + /* for AD-HOC mode */ + psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr); + if (psta != NULL) { + /* the sta have been in sta_info_queue => do nothing */ + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue\n")); + + return; /* between drv has received this event before and fw have not yet to set key to CAM_ENTRY) */ + } + + psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr); + if (psta == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't alloc sta_info when rtw_stassoc_event_callback\n")); + return; + } + + /* to do : init sta_info variable */ + psta->qos_option = 0; + psta->mac_id = (uint)pstassoc->cam_id; + /* psta->aid = (uint)pstassoc->cam_id; */ + DBG_871X("%s\n", __func__); + /* for ad-hoc mode */ + rtw_hal_set_odm_var(adapter, HAL_ODM_STA_INFO, psta, true); + + rtw_sta_media_status_rpt(adapter, psta, 1); + + if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm; + + + psta->ieee8021x_blocked = false; + + spin_lock_bh(&pmlmepriv->lock); + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == 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_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + pmlmepriv->cur_network_scanned = ptarget_wlan; + if (ptarget_wlan) + ptarget_wlan->fixed = true; + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + /* a sta + bc/mc_stainfo (not Ibss_stainfo) */ + rtw_indicate_connect(adapter); + } + } + + spin_unlock_bh(&pmlmepriv->lock); + + + mlmeext_sta_add_event_callback(adapter, psta); +} + +void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf) +{ + int mac_id = (-1); + struct sta_info *psta; + struct wlan_network *pwlan = NULL; + struct wlan_bssid_ex *pdev_network = NULL; + u8 *pibss = NULL; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct stadel_event *pstadel = (struct stadel_event *)pbuf; + struct wlan_network *tgt_network = &(pmlmepriv->cur_network); + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr); + if (psta) + mac_id = psta->mac_id; + else + mac_id = pstadel->mac_id; + + DBG_871X("%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_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status); + } + + /* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */ + if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) + return; + + + mlmeext_sta_del_event_callback(adapter); + + spin_lock_bh(&pmlmepriv->lock); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + u16 reason = *((unsigned short *)(pstadel->rsvd)); + bool roam = false; + struct wlan_network *roam_target = NULL; + + if (adapter->registrypriv.wifi_spec == 1) { + roam = false; + } else if (reason == WLAN_REASON_EXPIRATION_CHK && rtw_chk_roam_flags(adapter, RTW_ROAM_ON_EXPIRED)) { + roam = true; + } else if (reason == WLAN_REASON_ACTIVE_ROAM && rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) { + roam = true; + roam_target = pmlmepriv->roam_network; + } +#ifdef CONFIG_INTEL_WIDI + else if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_CONNECTED) { + roam = true; + } +#endif /* CONFIG_INTEL_WIDI */ + + if (roam == true) { + if (rtw_to_roam(adapter) > 0) + rtw_dec_to_roam(adapter); /* this stadel_event is caused by roaming, decrease to_roam */ + else if (rtw_to_roam(adapter) == 0) + rtw_set_to_roam(adapter, adapter->registrypriv.max_roaming_times); + } else { + rtw_set_to_roam(adapter, 0); + } + + rtw_free_uc_swdec_pending_queue(adapter); + + rtw_free_assoc_resources(adapter, 1); + rtw_indicate_disconnect(adapter); + + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + /* remove the network entry in scanned_queue */ + pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress); + if (pwlan) { + pwlan->fixed = false; + rtw_free_network_nolock(adapter, pwlan); + } + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + +#ifdef CONFIG_INTEL_WIDI + if (!rtw_to_roam(adapter)) + process_intel_widi_disconnect(adapter, 1); +#endif /* CONFIG_INTEL_WIDI */ + + _rtw_roaming(adapter, roam_target); + } + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) || + check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) { + + rtw_free_stainfo(adapter, psta); + + if (adapter->stapriv.asoc_sta_count == 1) {/* a sta + bc/mc_stainfo (not Ibss_stainfo) */ + /* rtw_indicate_disconnect(adapter);removed@20091105 */ + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + /* free old ibss network */ + /* pwlan = rtw_find_network(&pmlmepriv->scanned_queue, pstadel->macaddr); */ + pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress); + if (pwlan) { + pwlan->fixed = false; + rtw_free_network_nolock(adapter, pwlan); + } + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + /* re-create ibss */ + pdev_network = &(adapter->registrypriv.dev_network); + pibss = adapter->registrypriv.dev_network.MacAddress; + + memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network)); + + memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid)); + memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid)); + + rtw_update_registrypriv_dev_network(adapter); + + rtw_generate_random_ibss(pibss); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) { + set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE); + } + + if (rtw_createbss_cmd(adapter) != _SUCCESS) { + + RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd status FAIL***\n ")); + + } + + + } + + } + + spin_unlock_bh(&pmlmepriv->lock); +} + +void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf) +{ + struct reportpwrstate_parm *preportpwrstate; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_cpwm_event_callback !!!\n")); + preportpwrstate = (struct reportpwrstate_parm *)pbuf; + preportpwrstate->state |= (u8)(adapter_to_pwrctl(padapter)->cpwm_tog + 0x80); + cpwm_int_hdl(padapter, preportpwrstate); +} + + +void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf) +{ + WMMOnAssocRsp(padapter); +} + +/* +* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss +* @adapter: pointer to struct adapter structure +*/ +void _rtw_join_timeout_handler (struct adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + DBG_871X("%s, fw_state =%x\n", __func__, get_fwstate(pmlmepriv)); + + if (adapter->bDriverStopped || adapter->bSurpriseRemoved) + return; + + spin_lock_bh(&pmlmepriv->lock); + + if (rtw_to_roam(adapter) > 0) { /* join timeout caused by roaming */ + while (1) { + rtw_dec_to_roam(adapter); + if (rtw_to_roam(adapter) != 0) { /* try another */ + int do_join_r; + DBG_871X("%s try another roaming\n", __func__); + do_join_r = rtw_do_join(adapter); + if (_SUCCESS != do_join_r) { + DBG_871X("%s roaming do_join return %d\n", __func__, do_join_r); + continue; + } + break; + } else { +#ifdef CONFIG_INTEL_WIDI + if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) { + memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN); + intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0); + DBG_871X("change to widi listen\n"); + } +#endif /* CONFIG_INTEL_WIDI */ + DBG_871X("%s We've try roaming but fail\n", __func__); + rtw_indicate_disconnect(adapter); + break; + } + } + + } else{ + rtw_indicate_disconnect(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_handler - Timeout/Faliure handler for CMD SiteSurvey +* @adapter: pointer to struct adapter structure +*/ +void rtw_scan_timeout_handler (struct adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + DBG_871X(FUNC_ADPT_FMT" fw_state =%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv)); + + spin_lock_bh(&pmlmepriv->lock); + + _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY); + + spin_unlock_bh(&pmlmepriv->lock); + + rtw_indicate_scan_done(adapter, true); +} + +void rtw_mlme_reset_auto_scan_int(struct adapter *adapter) +{ + struct mlme_priv *mlme = &adapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pmlmeinfo->VHT_enable) /* disable auto scan when connect to 11AC AP */ + mlme->auto_scan_int_ms = 0; + else if (adapter->registrypriv.wifi_spec && is_client_associated_to_ap(adapter) == true) + mlme->auto_scan_int_ms = 60*1000; + else if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) { + if (check_fwstate(mlme, WIFI_STATION_STATE) && check_fwstate(mlme, _FW_LINKED)) + mlme->auto_scan_int_ms = mlme->roam_scan_int_ms; + } else + mlme->auto_scan_int_ms = 0; /* disabled */ + + return; +} + +static void rtw_auto_scan_handler(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + rtw_mlme_reset_auto_scan_int(padapter); + + if (pmlmepriv->auto_scan_int_ms != 0 + && jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time) > pmlmepriv->auto_scan_int_ms) { + + if (!padapter->registrypriv.wifi_spec) { + if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) { + DBG_871X(FUNC_ADPT_FMT" _FW_UNDER_SURVEY|_FW_UNDER_LINKING\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true) { + DBG_871X(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + } + + DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + rtw_set_802_11_bssid_list_scan(padapter, NULL, 0); + } + +exit: + return; +} + +void rtw_dynamic_check_timer_handlder(struct adapter *adapter) +{ + if (!adapter) + return; + + if (adapter->hw_init_completed == false) + return; + + if ((adapter->bDriverStopped == true) || (adapter->bSurpriseRemoved == true)) + return; + + if (adapter->net_closed == true) + return; + + if (is_primary_adapter(adapter)) + DBG_871X("IsBtDisabled =%d, IsBtControlLps =%d\n", rtw_btcoex_IsBtDisabled(adapter), rtw_btcoex_IsBtControlLps(adapter)); + + if ((adapter_to_pwrctl(adapter)->bFwCurrentInPSMode == true) + && (rtw_btcoex_IsBtControlLps(adapter) == false) + ) { + u8 bEnterPS; + + linked_status_chk(adapter); + + bEnterPS = traffic_status_watchdog(adapter, 1); + if (bEnterPS) { + /* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 1); */ + rtw_hal_dm_watchdog_in_lps(adapter); + } else{ + /* call rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1) in traffic_status_watchdog() */ + } + + } else{ + if (is_primary_adapter(adapter)) { + rtw_dynamic_chk_wk_cmd(adapter); + } + } + + /* auto site survey */ + rtw_auto_scan_handler(adapter); +} + + +inline bool rtw_is_scan_deny(struct adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + return (atomic_read(&mlmepriv->set_scan_deny) != 0) ? true : false; +} + +inline void rtw_clear_scan_deny(struct adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + atomic_set(&mlmepriv->set_scan_deny, 0); + + DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); +} + +void rtw_set_scan_deny_timer_hdl(struct adapter *adapter) +{ + rtw_clear_scan_deny(adapter); +} + +void rtw_set_scan_deny(struct adapter *adapter, u32 ms) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + + DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); + atomic_set(&mlmepriv->set_scan_deny, 1); + _set_timer(&mlmepriv->set_scan_deny_timer, ms); +} + +/* +* Select a new roaming candidate from the original @param candidate and @param competitor +* @return true: candidate is updated +* @return false: candidate is not updated +*/ +static int rtw_check_roaming_candidate(struct mlme_priv *mlme + , struct wlan_network **candidate, struct wlan_network *competitor) +{ + int updated = false; + struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv); + + if (is_same_ess(&competitor->network, &mlme->cur_network.network) == false) + goto exit; + + if (rtw_is_desired_network(adapter, competitor) == false) + goto exit; + + DBG_871X("roam candidate:%s %s("MAC_FMT", ch%3u) rssi:%d, age:%5d\n", + (competitor == mlme->cur_network_scanned)?"*":" ", + competitor->network.Ssid.Ssid, + MAC_ARG(competitor->network.MacAddress), + competitor->network.Configuration.DSConfig, + (int)competitor->network.Rssi, + jiffies_to_msecs(jiffies - competitor->last_scanned) + ); + + /* got specific addr to roam */ + if (!is_zero_mac_addr(mlme->roam_tgt_addr)) { + if (!memcmp(mlme->roam_tgt_addr, competitor->network.MacAddress, ETH_ALEN)) + goto update; + else + goto exit; + } + if (jiffies_to_msecs(jiffies - competitor->last_scanned) >= mlme->roam_scanr_exp_ms) + goto exit; + + if (competitor->network.Rssi - mlme->cur_network_scanned->network.Rssi < mlme->roam_rssi_diff_th) + goto exit; + + if (*candidate != NULL && (*candidate)->network.Rssi >= competitor->network.Rssi) + goto exit; + +update: + *candidate = competitor; + updated = true; + +exit: + return updated; +} + +int rtw_select_roaming_candidate(struct mlme_priv *mlme) +{ + int ret = _FAIL; + struct list_head *phead; + struct adapter *adapter; + struct __queue *queue = &(mlme->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *candidate = NULL; + + if (mlme->cur_network_scanned == NULL) { + rtw_warn_on(1); + return ret; + } + + spin_lock_bh(&(mlme->scanned_queue.lock)); + phead = get_list_head(queue); + adapter = (struct adapter *)mlme->nic_hdl; + + mlme->pscanned = get_next(phead); + + while (phead != mlme->pscanned) { + + pnetwork = LIST_CONTAINOR(mlme->pscanned, struct wlan_network, list); + if (pnetwork == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__)); + ret = _FAIL; + goto exit; + } + + mlme->pscanned = get_next(mlme->pscanned); + + DBG_871X("%s("MAC_FMT", ch%u) rssi:%d\n" + , pnetwork->network.Ssid.Ssid + , MAC_ARG(pnetwork->network.MacAddress) + , pnetwork->network.Configuration.DSConfig + , (int)pnetwork->network.Rssi); + + rtw_check_roaming_candidate(mlme, &candidate, pnetwork); + + } + + if (candidate == NULL) { + DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__); + ret = _FAIL; + goto exit; + } else { + DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__, + candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig); + + mlme->roam_network = candidate; + + if (!memcmp(candidate->network.MacAddress, mlme->roam_tgt_addr, ETH_ALEN)) + memset(mlme->roam_tgt_addr, 0, ETH_ALEN); + } + + ret = _SUCCESS; +exit: + spin_unlock_bh(&(mlme->scanned_queue.lock)); + + return ret; +} + +/* +* 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 *mlme + , struct wlan_network **candidate, struct wlan_network *competitor) +{ + int updated = false; + struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv); + + + /* check bssid, if needed */ + if (mlme->assoc_by_bssid == true) { + if (memcmp(competitor->network.MacAddress, mlme->assoc_bssid, ETH_ALEN)) + goto exit; + } + + /* check ssid, if needed */ + if (mlme->assoc_ssid.Ssid[0] && mlme->assoc_ssid.SsidLength) { + if (competitor->network.Ssid.SsidLength != mlme->assoc_ssid.SsidLength + || memcmp(competitor->network.Ssid.Ssid, mlme->assoc_ssid.Ssid, mlme->assoc_ssid.SsidLength) + ) + goto exit; + } + + if (rtw_is_desired_network(adapter, competitor) == false) + goto exit; + + if (rtw_to_roam(adapter) > 0) { + if (jiffies_to_msecs(jiffies - competitor->last_scanned) >= mlme->roam_scanr_exp_ms + || is_same_ess(&competitor->network, &mlme->cur_network.network) == false + ) + goto exit; + } + + if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) { + *candidate = competitor; + updated = true; + } + + if (updated) { + DBG_871X("[by_bssid:%u][assoc_ssid:%s]" + "[to_roam:%u] " + "new candidate: %s("MAC_FMT", ch%u) rssi:%d\n", + mlme->assoc_by_bssid, + mlme->assoc_ssid.Ssid, + rtw_to_roam(adapter), + (*candidate)->network.Ssid.Ssid, + MAC_ARG((*candidate)->network.MacAddress), + (*candidate)->network.Configuration.DSConfig, + (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_queue(struct mlme_priv *pmlmepriv) +{ + int ret; + struct list_head *phead; + struct adapter *adapter; + struct __queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *candidate = NULL; + + adapter = (struct adapter *)pmlmepriv->nic_hdl; + + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + + if (pmlmepriv->roam_network) { + candidate = pmlmepriv->roam_network; + pmlmepriv->roam_network = NULL; + goto candidate_exist; + } + + phead = get_list_head(queue); + pmlmepriv->pscanned = get_next(phead); + + while (phead != pmlmepriv->pscanned) { + + pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list); + if (pnetwork == NULL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__)); + ret = _FAIL; + goto exit; + } + + pmlmepriv->pscanned = get_next(pmlmepriv->pscanned); + + DBG_871X("%s("MAC_FMT", ch%u) rssi:%d\n" + , pnetwork->network.Ssid.Ssid + , MAC_ARG(pnetwork->network.MacAddress) + , pnetwork->network.Configuration.DSConfig + , (int)pnetwork->network.Rssi); + + rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork); + + } + + if (candidate == NULL) { + DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__); +#ifdef CONFIG_WOWLAN + _clr_fwstate_(pmlmepriv, _FW_LINKED|_FW_UNDER_LINKING); +#endif + ret = _FAIL; + goto exit; + } else { + DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__, + candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig); + goto candidate_exist; + } + +candidate_exist: + + /* check for situation of _FW_LINKED */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + DBG_871X("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__); + + rtw_disassoc_cmd(adapter, 0, true); + rtw_indicate_disconnect(adapter); + rtw_free_assoc_resources(adapter, 0); + } + + set_fwstate(pmlmepriv, _FW_UNDER_LINKING); + ret = rtw_joinbss_cmd(adapter, candidate); + +exit: + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + return ret; +} + +sint rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv) +{ + struct cmd_obj *pcmd; + struct setauth_parm *psetauthparm; + struct cmd_priv *pcmdpriv = &(adapter->cmdpriv); + sint res = _SUCCESS; + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; /* try again */ + goto exit; + } + + psetauthparm = (struct setauth_parm *)rtw_zmalloc(sizeof(struct setauth_parm)); + if (psetauthparm == NULL) { + kfree((unsigned char *)pcmd); + res = _FAIL; + goto exit; + } + + memset(psetauthparm, 0, sizeof(struct setauth_parm)); + psetauthparm->mode = (unsigned char)psecuritypriv->dot11AuthAlgrthm; + + pcmd->cmdcode = _SetAuth_CMD_; + pcmd->parmbuf = (unsigned char *)psetauthparm; + pcmd->cmdsz = (sizeof(struct setauth_parm)); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + + INIT_LIST_HEAD(&pcmd->list); + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("after enqueue set_auth_cmd, auth_mode =%x\n", psecuritypriv->dot11AuthAlgrthm)); + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + return res; +} + +sint rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, sint keyid, u8 set_tx, bool enqueue) +{ + u8 keylen; + struct cmd_obj *pcmd; + struct setkey_parm *psetkeyparm; + struct cmd_priv *pcmdpriv = &(adapter->cmdpriv); + sint res = _SUCCESS; + + psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm)); + if (psetkeyparm == NULL) { + res = _FAIL; + goto exit; + } + memset(psetkeyparm, 0, sizeof(struct setkey_parm)); + + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(unsigned char)psecuritypriv->dot118021XGrpPrivacy =%d\n", psetkeyparm->algorithm)); + } else { + psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(u8)psecuritypriv->dot11PrivacyAlgrthm =%d\n", psetkeyparm->algorithm)); + + } + psetkeyparm->keyid = (u8)keyid;/* 0~3 */ + psetkeyparm->set_tx = set_tx; + if (is_wep_enc(psetkeyparm->algorithm)) + adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid); + + DBG_871X("==> rtw_set_key algorithm(%x), keyid(%x), key_mask(%x)\n", psetkeyparm->algorithm, psetkeyparm->keyid, adapter->securitypriv.key_mask); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =%d psetkeyparm->keyid =(u8)keyid =%d\n", psetkeyparm->algorithm, keyid)); + + switch (psetkeyparm->algorithm) { + + case _WEP40_: + keylen = 5; + memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen); + break; + case _WEP104_: + keylen = 13; + memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen); + break; + case _TKIP_: + keylen = 16; + memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen); + psetkeyparm->grpkey = 1; + break; + case _AES_: + keylen = 16; + memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen); + psetkeyparm->grpkey = 1; + break; + default: + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm = %x (must be 1 or 2 or 4 or 5)\n", psecuritypriv->dot11PrivacyAlgrthm)); + res = _FAIL; + kfree((unsigned char *)psetkeyparm); + goto exit; + } + + + if (enqueue) { + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + kfree((unsigned char *)psetkeyparm); + res = _FAIL; /* try again */ + 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_cmd(pcmdpriv, pcmd); + } else{ + setkey_hdl(adapter, (u8 *)psetkeyparm); + kfree((u8 *) psetkeyparm); + } +exit: + return res; +} + +/* adjust IEs for rtw_joinbss_cmd in WMM */ +int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len) +{ + unsigned int ielength = 0; + unsigned int i, j; + + i = 12; /* after the fixed IE */ + while (i < in_len) { + ielength = initial_out_len; + + if (in_ie[i] == 0xDD && in_ie[i+2] == 0x00 && in_ie[i+3] == 0x50 && in_ie[i+4] == 0xF2 && in_ie[i+5] == 0x02 && i+5 < in_len) { /* WMM element ID and OUI */ + for (j = i; j < i + 9; j++) { + out_ie[ielength] = in_ie[j]; + ielength++; + } + out_ie[initial_out_len + 1] = 0x07; + out_ie[initial_out_len + 6] = 0x00; + out_ie[initial_out_len + 8] = 0x00; + + break; + } + + i += (in_ie[i+1]+2); /* to the next IE element */ + } + + return ielength; + +} + + +/* */ +/* Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.) */ +/* Added by Annie, 2006-05-07. */ +/* */ +/* Search by BSSID, */ +/* Return Value: */ +/* -1 :if there is no pre-auth key in the table */ +/* >= 0 :if there is pre-auth key, and return the entry id */ +/* */ +/* */ + +static int SecIsInPMKIDList(struct adapter *Adapter, u8 *bssid) +{ + struct security_priv *psecuritypriv = &Adapter->securitypriv; + int i = 0; + + do { + if ((psecuritypriv->PMKIDList[i].bUsed) && + (!memcmp(psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN))) { + break; + } else{ + i++; + /* continue; */ + } + + } while (i < NUM_PMKID_CACHE); + + if (i == NUM_PMKID_CACHE) { + i = -1;/* Could not find. */ + } else { + /* There is one Pre-Authentication Key for the specific BSSID. */ + } + + return i; + +} + +/* */ +/* Check the RSN IE length */ +/* If the RSN IE length <= 20, the RSN IE didn't include the PMKID information */ +/* 0-11th element in the array are the fixed IE */ +/* 12th element in the array is the IE */ +/* 13th element in the array is the IE length */ +/* */ + +static int rtw_append_pmkid(struct adapter *Adapter, int iEntry, u8 *ie, uint ie_len) +{ + struct security_priv *psecuritypriv = &Adapter->securitypriv; + + if (ie[13] <= 20) { + /* The RSN IE didn't include the PMK ID, append the PMK information */ + ie[ie_len] = 1; + ie_len++; + ie[ie_len] = 0; /* PMKID count = 0x0100 */ + ie_len++; + memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16); + + ie_len += 16; + ie[13] += 18;/* PMKID length = 2+16 */ + + } + return ie_len; +} + +sint rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len) +{ + u8 authmode = 0x0; + uint ielength; + int iEntry; + + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct security_priv *psecuritypriv = &adapter->securitypriv; + uint ndisauthmode = psecuritypriv->ndisauthtype; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, + ("+rtw_restruct_sec_ie: ndisauthmode =%d ndissecuritytype =%d\n", + ndisauthmode, ndissecuritytype)); + + /* copy fixed ie only */ + memcpy(out_ie, in_ie, 12); + ielength = 12; + if ((ndisauthmode == Ndis802_11AuthModeWPA) || (ndisauthmode == Ndis802_11AuthModeWPAPSK)) + authmode = _WPA_IE_ID_; + if ((ndisauthmode == Ndis802_11AuthModeWPA2) || (ndisauthmode == Ndis802_11AuthModeWPA2PSK)) + authmode = _WPA2_IE_ID_; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + memcpy(out_ie+ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len); + + ielength += psecuritypriv->wps_ie_len; + } else if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) { + /* copy RSN or SSN */ + memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1]+2); + /* debug for CONFIG_IEEE80211W + { + int jj; + printk("supplicant_ie_length =%d &&&&&&&&&&&&&&&&&&&\n", psecuritypriv->supplicant_ie[1]+2); + for (jj = 0; jj < psecuritypriv->supplicant_ie[1]+2; jj++) + printk(" %02x ", psecuritypriv->supplicant_ie[jj]); + printk("\n"); + }*/ + ielength += psecuritypriv->supplicant_ie[1]+2; + rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie); + } + + iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid); + if (iEntry < 0) { + return ielength; + } else{ + if (authmode == _WPA2_IE_ID_) + ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength); + } + return ielength; +} + +void rtw_init_registrypriv_dev_network(struct adapter *adapter) +{ + struct registry_priv *pregistrypriv = &adapter->registrypriv; + struct eeprom_priv *peepriv = &adapter->eeprompriv; + struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network; + u8 *myhwaddr = myid(peepriv); + + memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN); + + memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid)); + + pdev_network->Configuration.Length = sizeof(struct ndis_802_11_conf); + pdev_network->Configuration.BeaconPeriod = 100; + pdev_network->Configuration.FHConfig.Length = 0; + pdev_network->Configuration.FHConfig.HopPattern = 0; + pdev_network->Configuration.FHConfig.HopSet = 0; + pdev_network->Configuration.FHConfig.DwellTime = 0; +} + +void rtw_update_registrypriv_dev_network(struct adapter *adapter) +{ + int sz = 0; + struct registry_priv *pregistrypriv = &adapter->registrypriv; + struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network; + struct security_priv *psecuritypriv = &adapter->securitypriv; + struct wlan_network *cur_network = &adapter->mlmepriv.cur_network; + /* struct xmit_priv *pxmitpriv = &adapter->xmitpriv; */ + + pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0) ; /* adhoc no 802.1x */ + + pdev_network->Rssi = 0; + + switch (pregistrypriv->wireless_mode) { + case WIRELESS_11B: + pdev_network->NetworkTypeInUse = (Ndis802_11DS); + break; + case WIRELESS_11G: + case WIRELESS_11BG: + case WIRELESS_11_24N: + case WIRELESS_11G_24N: + case WIRELESS_11BG_24N: + pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24); + break; + case WIRELESS_11A: + case WIRELESS_11A_5N: + pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5); + break; + case WIRELESS_11ABGN: + if (pregistrypriv->channel > 14) + pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5); + else + pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24); + break; + default: + /* TODO */ + break; + } + + pdev_network->Configuration.DSConfig = (pregistrypriv->channel); + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("pregistrypriv->channel =%d, pdev_network->Configuration.DSConfig = 0x%x\n", pregistrypriv->channel, pdev_network->Configuration.DSConfig)); + + if (cur_network->network.InfrastructureMode == Ndis802_11IBSS) + pdev_network->Configuration.ATIMWindow = (0); + + pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode); + + /* 1. Supported rates */ + /* 2. IE */ + + /* rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ; will be called in rtw_generate_ie */ + sz = rtw_generate_ie(pregistrypriv); + + pdev_network->IELength = sz; + + pdev_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network); + + /* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */ + /* pdev_network->IELength = cpu_to_le32(sz); */ +} + +void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter) +{ +} + +/* the fucntion is at passive_level */ +void rtw_joinbss_reset(struct adapter *padapter) +{ + u8 threshold; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + + /* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */ + + pmlmepriv->num_FortyMHzIntolerant = 0; + + pmlmepriv->num_sta_no_ht = 0; + + phtpriv->ampdu_enable = false;/* reset to disabled */ + + /* TH = 1 => means that invalidate usb rx aggregation */ + /* TH = 0 => means that validate usb rx aggregation, use init value. */ + if (phtpriv->ht_option) { + if (padapter->registrypriv.wifi_spec == 1) + threshold = 1; + else + threshold = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } else{ + threshold = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } +} + +void rtw_ht_use_default_setting(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + bool bHwLDPCSupport = false, bHwSTBCSupport = false; + bool bHwSupportBeamformer = false, bHwSupportBeamformee = false; + + if (pregistrypriv->wifi_spec) + phtpriv->bss_coexist = 1; + else + phtpriv->bss_coexist = 0; + + phtpriv->sgi_40m = TEST_FLAG(pregistrypriv->short_gi, BIT1) ? true : false; + phtpriv->sgi_20m = TEST_FLAG(pregistrypriv->short_gi, BIT0) ? true : false; + + /* LDPC support */ + rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport); + CLEAR_FLAGS(phtpriv->ldpc_cap); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT4)) + SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT5)) + SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX); + } + if (phtpriv->ldpc_cap) + DBG_871X("[HT] Support LDPC = 0x%02X\n", phtpriv->ldpc_cap); + + /* STBC */ + rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport); + CLEAR_FLAGS(phtpriv->stbc_cap); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT5)) + SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT4)) + SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX); + } + if (phtpriv->stbc_cap) + DBG_871X("[HT] Support STBC = 0x%02X\n", phtpriv->stbc_cap); + + /* Beamforming setting */ + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer); + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee); + CLEAR_FLAGS(phtpriv->beamform_cap); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT4) && bHwSupportBeamformer) { + SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + DBG_871X("[HT] Support Beamformer\n"); + } + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee) { + SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + DBG_871X("[HT] Support Beamformee\n"); + } +} + +void rtw_build_wmm_ie_ht(struct adapter *padapter, u8 *out_ie, uint *pout_len) +{ + unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00}; + int out_len; + u8 *pframe; + + if (padapter->mlmepriv.qospriv.qos_option == 0) { + out_len = *pout_len; + pframe = rtw_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_, + _WMM_IE_Length_, WMM_IE, pout_len); + + padapter->mlmepriv.qospriv.qos_option = 1; + } +} + +/* the fucntion is >= passive_level */ +unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel) +{ + u32 ielen, out_len; + enum HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor; + unsigned char *p, *pframe; + struct rtw_ieee80211_ht_cap ht_capie; + u8 cbw40_enable = 0, stbc_rx_enable = 0, rf_type = 0, operation_bw = 0; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + phtpriv->ht_option = false; + + out_len = *pout_len; + + memset(&ht_capie, 0, sizeof(struct rtw_ieee80211_ht_cap)); + + ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_DSSSCCK40); + + if (phtpriv->sgi_20m) + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_20); + + /* Get HT BW */ + if (in_ie == NULL) { + /* TDLS: TODO 20/40 issue */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + operation_bw = padapter->mlmeextpriv.cur_bwmode; + if (operation_bw > CHANNEL_WIDTH_40) + operation_bw = CHANNEL_WIDTH_40; + } else + /* TDLS: TODO 40? */ + operation_bw = CHANNEL_WIDTH_40; + } else { + p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len); + if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) { + struct HT_info_element *pht_info = (struct HT_info_element *)(p+2); + if (pht_info->infos[0] & BIT(2)) { + switch (pht_info->infos[0] & 0x3) { + case 1: + case 3: + operation_bw = CHANNEL_WIDTH_40; + break; + default: + operation_bw = CHANNEL_WIDTH_20; + break; + } + } else { + operation_bw = CHANNEL_WIDTH_20; + } + } + } + + /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */ + if (channel > 14) { + if ((pregistrypriv->bw_mode & 0xf0) > 0) + cbw40_enable = 1; + } else { + if ((pregistrypriv->bw_mode & 0x0f) > 0) + cbw40_enable = 1; + } + + if ((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) { + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH); + if (phtpriv->sgi_40m) + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_40); + } + + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX)) + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_TX_STBC); + + /* todo: disable SM power save mode */ + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS); + + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) { + if ((channel <= 14 && pregistrypriv->rx_stbc == 0x1) || /* enable for 2.4GHz */ + (pregistrypriv->wifi_spec == 1)) { + stbc_rx_enable = 1; + DBG_871X("declare supporting RX STBC\n"); + } + } + + /* fill default supported_mcs_set */ + memcpy(ht_capie.supp_mcs_set, pmlmeext->default_supported_mcs_set, 16); + + /* update default supported_mcs_set */ + rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + + switch (rf_type) { + case RF_1T1R: + if (stbc_rx_enable) + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_1R);/* RX STBC One spatial stream */ + + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_1R); + break; + + case RF_2T2R: + case RF_1T2R: + default: + if (stbc_rx_enable) + ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_2R);/* RX STBC two spatial stream */ + + #ifdef CONFIG_DISABLE_MCS13TO15 + if (((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) && (pregistrypriv->wifi_spec != 1)) + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R_13TO15_OFF); + else + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R); + #else /* CONFIG_DISABLE_MCS13TO15 */ + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R); + #endif /* CONFIG_DISABLE_MCS13TO15 */ + break; + } + + { + u32 rx_packet_offset, max_recvbuf_sz; + rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset); + rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz); + } + + if (padapter->driver_rx_ampdu_factor != 0xFF) + max_rx_ampdu_factor = + (enum HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor; + else + rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, + &max_rx_ampdu_factor); + + /* rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); */ + ht_capie.ampdu_params_info = (max_rx_ampdu_factor&0x03); + + if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_) + ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2)); + else + ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00); + + pframe = rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_, + sizeof(struct rtw_ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len); + + phtpriv->ht_option = true; + + if (in_ie != NULL) { + p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len); + if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) { + out_len = *pout_len; + pframe = rtw_set_ie(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_cap(struct adapter *padapter, u8 *pie, uint ie_len, u8 channel) +{ + u8 *p, max_ampdu_sz; + int len; + /* struct sta_info *bmc_sta, *psta; */ + struct rtw_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); + u8 cbw40_enable = 0; + + + if (!phtpriv->ht_option) + return; + + if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable)) + return; + + DBG_871X("+rtw_update_ht_cap()\n"); + + /* maybe needs check if ap supports rx ampdu. */ + if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) { + if (pregistrypriv->wifi_spec == 1) { + /* remove this part because testbed AP should disable RX AMPDU */ + /* phtpriv->ampdu_enable = false; */ + phtpriv->ampdu_enable = true; + } else { + phtpriv->ampdu_enable = true; + } + } else if (pregistrypriv->ampdu_enable == 2) { + /* remove this part because testbed AP should disable RX AMPDU */ + /* phtpriv->ampdu_enable = true; */ + } + + + /* check Max Rx A-MPDU Size */ + len = 0; + p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie)); + if (p && len > 0) { + pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2); + max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR); + max_ampdu_sz = 1 << (max_ampdu_sz+3); /* max_ampdu_sz (kbytes); */ + + /* DBG_871X("rtw_update_ht_cap(): max_ampdu_sz =%d\n", max_ampdu_sz); */ + phtpriv->rx_ampdu_maxlen = max_ampdu_sz; + + } + + + len = 0; + p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_ADD_INFO_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie)); + if (p && len > 0) { + pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2); + /* todo: */ + } + + if (channel > 14) { + if ((pregistrypriv->bw_mode & 0xf0) > 0) + cbw40_enable = 1; + } else { + if ((pregistrypriv->bw_mode & 0x0f) > 0) + cbw40_enable = 1; + } + + /* update cur_bwmode & cur_ch_offset */ + if ((cbw40_enable) && + (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & + BIT(1)) && (pmlmeinfo->HT_info.infos[0] & BIT(2))) { + int i; + u8 rf_type; + + rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + + /* update the MCS set */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i]; + + /* update the MCS rates */ + switch (rf_type) { + case RF_1T1R: + case RF_1T2R: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R); + break; + case RF_2T2R: + default: +#ifdef CONFIG_DISABLE_MCS13TO15 + if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1) + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF); + else + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); +#else /* CONFIG_DISABLE_MCS13TO15 */ + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); +#endif /* CONFIG_DISABLE_MCS13TO15 */ + } + + /* switch to the 40M Hz mode according to the AP */ + /* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */ + switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) { + case EXTCHNL_OFFSET_UPPER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + + case EXTCHNL_OFFSET_LOWER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } + + /* */ + /* Config SM Power Save setting */ + /* */ + pmlmeinfo->SM_PS = + (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & + 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) + DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__); + + /* */ + /* Config current HT Protection mode. */ + /* */ + pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; +} + +void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + u8 issued; + int priority; + struct sta_info *psta = NULL; + struct ht_priv *phtpriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + s32 bmcst = IS_MCAST(pattrib->ra); + + /* if (bmcst || (padapter->mlmepriv.LinkDetectInfo.bTxBusyTraffic == false)) */ + if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100)) + return; + + priority = pattrib->priority; + + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return; + } + + if (psta == NULL) { + DBG_871X("%s, psta ==NUL\n", __func__); + return; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_871X("%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_871X("rtw_issue_addbareq_cmd, p =%d\n", priority); + psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority); + rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra); + } + } + +} + +void rtw_append_exented_cap(struct adapter *padapter, u8 *out_ie, uint *pout_len) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + u8 cap_content[8] = {0}; + u8 *pframe; + + + if (phtpriv->bss_coexist) { + SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1); + } + + pframe = rtw_set_ie(out_ie + *pout_len, EID_EXTCapability, 8, cap_content, pout_len); +} + +inline void rtw_set_to_roam(struct adapter *adapter, u8 to_roam) +{ + if (to_roam == 0) + adapter->mlmepriv.to_join = false; + adapter->mlmepriv.to_roam = to_roam; +} + +inline u8 rtw_dec_to_roam(struct adapter *adapter) +{ + adapter->mlmepriv.to_roam--; + return adapter->mlmepriv.to_roam; +} + +inline u8 rtw_to_roam(struct adapter *adapter) +{ + return adapter->mlmepriv.to_roam; +} + +void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + spin_lock_bh(&pmlmepriv->lock); + _rtw_roaming(padapter, tgt_network); + spin_unlock_bh(&pmlmepriv->lock); +} +void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *cur_network = &pmlmepriv->cur_network; + int do_join_r; + + if (0 < rtw_to_roam(padapter)) { + DBG_871X("roaming from %s("MAC_FMT"), length:%d\n", + cur_network->network.Ssid.Ssid, MAC_ARG(cur_network->network.MacAddress), + cur_network->network.Ssid.SsidLength); + memcpy(&pmlmepriv->assoc_ssid, &cur_network->network.Ssid, sizeof(struct ndis_802_11_ssid)); + + pmlmepriv->assoc_by_bssid = false; + + while (1) { + do_join_r = rtw_do_join(padapter); + if (_SUCCESS == do_join_r) { + break; + } else { + DBG_871X("roaming do_join return %d\n", do_join_r); + rtw_dec_to_roam(padapter); + + if (rtw_to_roam(padapter) > 0) { + continue; + } else { + DBG_871X("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__); + rtw_indicate_disconnect(padapter); + break; + } + } + } + } + +} + +sint rtw_linked_check(struct adapter *padapter) +{ + if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true) || + (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true)) { + 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/rtl8723bs/core/rtw_mlme_ext.c b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c new file mode 100644 index 0000000..17d881d --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c @@ -0,0 +1,6941 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <rtw_wifi_regd.h> + + +static struct mlme_handler mlme_sta_tbl[] = { + {WIFI_ASSOCREQ, "OnAssocReq", &OnAssocReq}, + {WIFI_ASSOCRSP, "OnAssocRsp", &OnAssocRsp}, + {WIFI_REASSOCREQ, "OnReAssocReq", &OnAssocReq}, + {WIFI_REASSOCRSP, "OnReAssocRsp", &OnAssocRsp}, + {WIFI_PROBEREQ, "OnProbeReq", &OnProbeReq}, + {WIFI_PROBERSP, "OnProbeRsp", &OnProbeRsp}, + + /*---------------------------------------------------------- + below 2 are reserved + -----------------------------------------------------------*/ + {0, "DoReserved", &DoReserved}, + {0, "DoReserved", &DoReserved}, + {WIFI_BEACON, "OnBeacon", &OnBeacon}, + {WIFI_ATIM, "OnATIM", &OnAtim}, + {WIFI_DISASSOC, "OnDisassoc", &OnDisassoc}, + {WIFI_AUTH, "OnAuth", &OnAuthClient}, + {WIFI_DEAUTH, "OnDeAuth", &OnDeAuth}, + {WIFI_ACTION, "OnAction", &OnAction}, + {WIFI_ACTION_NOACK, "OnActionNoAck", &OnAction}, +}; + +static struct action_handler OnAction_tbl[] = { + {RTW_WLAN_CATEGORY_SPECTRUM_MGMT, "ACTION_SPECTRUM_MGMT", on_action_spct}, + {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &DoReserved}, + {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &DoReserved}, + {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back}, + {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public}, + {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved}, + {RTW_WLAN_CATEGORY_FT, "ACTION_FT", &DoReserved}, + {RTW_WLAN_CATEGORY_HT, "ACTION_HT", &OnAction_ht}, + {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query}, + {RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved}, + {RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved}, + {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &DoReserved}, + {RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &DoReserved}, + {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &DoReserved}, +}; + + +static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + +/************************************************** +OUI definitions for the vendor specific IE +***************************************************/ +unsigned char RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01}; +unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02}; +unsigned char WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04}; +unsigned char P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09}; +unsigned char WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A}; + +unsigned char WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01}; +unsigned char WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + +static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20}; + +/******************************************************** +ChannelPlan definitions +*********************************************************/ +static 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 */ + {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14}, /* 0x05, RT_CHANNEL_DOMAIN_2G_GLOBAL , Passive scan CH 12, 13, 14 */ + {{}, 0}, /* 0x06, RT_CHANNEL_DOMAIN_2G_NULL */ +}; + +static 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 */ + {{149, 153, 157, 161, 165}, 5}, /* 0x11, RT_CHANNEL_DOMAIN_5G_NCC3 */ + {{36, 40, 44, 48}, 4}, /* 0x12, RT_CHANNEL_DOMAIN_5G_ETSI4 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20}, /* 0x13, RT_CHANNEL_DOMAIN_5G_ETSI5 */ + {{149, 153, 157, 161}, 4}, /* 0x14, RT_CHANNEL_DOMAIN_5G_FCC8 */ + {{36, 40, 44, 48, 52, 56, 60, 64}, 8}, /* 0x15, RT_CHANNEL_DOMAIN_5G_ETSI6 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13}, /* 0x16, RT_CHANNEL_DOMAIN_5G_ETSI7 */ + {{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9}, /* 0x17, RT_CHANNEL_DOMAIN_5G_ETSI8 */ + {{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11}, /* 0x18, RT_CHANNEL_DOMAIN_5G_ETSI9 */ + {{149, 153, 157, 161, 165}, 5}, /* 0x19, RT_CHANNEL_DOMAIN_5G_ETSI10 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 149, 153, 157, 161, 165}, 16}, /* 0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11 */ + {{52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17}, /* 0x1B, RT_CHANNEL_DOMAIN_5G_NCC4 */ + {{149, 153, 157, 161}, 4}, /* 0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12 */ + {{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17}, /* 0x1D, RT_CHANNEL_DOMAIN_5G_FCC9 */ + {{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140}, 12}, /* 0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13 */ + {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161}, 20}, /* 0x1F, RT_CHANNEL_DOMAIN_5G_FCC10 */ + + /* 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}, /* 0x20, RT_CHANNEL_DOMAIN_5G_FCC */ + {{36, 40, 44, 48}, 4}, /* 0x21, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */ + {{36, 40, 44, 48, 149, 153, 157, 161}, 8}, /* 0x22, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */ +}; + +static RT_CHANNEL_PLAN_MAP RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = { + /* 0x00 ~ 0x1F , Old Define ===== */ + {0x02, 0x20}, /* 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, 0x21}, /* 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, 0x21}, /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */ + {0x00, 0x22}, /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */ + {0x03, 0x21}, /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */ + {0x06, 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, */ + {0x06, 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 */ + {0x05, 0x00}, /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL */ + {0x01, 0x12}, /* 0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4 */ + {0x02, 0x05}, /* 0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2 */ + {0x02, 0x11}, /* 0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3 */ + {0x00, 0x13}, /* 0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5 */ + {0x02, 0x14}, /* 0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8 */ + {0x00, 0x15}, /* 0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6 */ + {0x00, 0x16}, /* 0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7 */ + {0x00, 0x17}, /* 0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8 */ + {0x00, 0x18}, /* 0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9 */ + {0x00, 0x19}, /* 0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10 */ + {0x00, 0x1A}, /* 0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11 */ + {0x02, 0x1B}, /* 0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4 */ + {0x00, 0x1C}, /* 0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12 */ + {0x02, 0x1D}, /* 0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9 */ + {0x00, 0x1E}, /* 0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13 */ + {0x02, 0x1F}, /* 0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10 */ +}; + +static RT_CHANNEL_PLAN_MAP RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */ + +/* + * Search the @param ch in given @param ch_set + * @ch_set: the given channel set + * @ch: the given channel number + * + * return the index of channel_num in channel_set, -1 if not found + */ +int rtw_ch_set_search_ch(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; +} + +/* + * Check the @param ch is fit with setband setting of @param adapter + * @adapter: the given adapter + * @ch: the given channel number + * + * return true when check valid, false not valid + */ +bool rtw_mlme_band_check(struct adapter *adapter, const u32 ch) +{ + if (adapter->setband == GHZ24_50 /* 2.4G and 5G */ + || (adapter->setband == GHZ_24 && ch < 35) /* 2.4G only */ + || (adapter->setband == GHZ_50 && ch > 35) /* 5G only */ + ) { + return true; + } + return false; +} + +/**************************************************************************** + +Following are the initialization functions for WiFi MLME + +*****************************************************************************/ + +int init_hw_mlme_ext(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + return _SUCCESS; +} + +void init_mlme_default_rate_set(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + 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,}; + unsigned char supported_mcs_set[16] = {0xff, 0xff, 0x00, 0x00, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + + memcpy(pmlmeext->datarate, mixed_datarate, NumRates); + memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates); + + memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set)); +} + +static void init_mlme_ext_priv_value(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + atomic_set(&pmlmeext->event_seq, 0); + pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */ + pmlmeext->sa_query_seq = 0; + pmlmeext->mgnt_80211w_IPN = 0; + pmlmeext->mgnt_80211w_IPN_rx = 0; + pmlmeext->cur_channel = padapter->registrypriv.channel; + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + pmlmeext->retry = 0; + + pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode; + + init_mlme_default_rate_set(padapter); + + 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(RT_CHANNEL_INFO *channel_set, + u8 chanset_size, + u8 chan) { + int i; + + for (i = 0; i < chanset_size; i++) { + if (channel_set[i].ChannelNum == chan) { + return 1; + } + } + + return 0; +} + +static void init_channel_list(struct adapter *padapter, 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) && (8 == o->inc)) + continue; + + if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) && + ((BW40MINUS == o->bw) || (BW40PLUS == o->bw))) + continue; + + if (reg == NULL) { + reg = &channel_list->reg_class[cla]; + cla++; + reg->reg_class = o->op_class; + reg->channels = 0; + } + reg->channel[reg->channels] = ch; + reg->channels++; + } + } + channel_list->reg_classes = cla; + +} + +static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, 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(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM); + + if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) { + DBG_871X("ChannelPlan ID %x error !!!!!\n", ChannelPlan); + return chanset_size; + } + + if (IsSupported24G(padapter->registrypriv.wireless_mode)) { + b2_4GBand = true; + if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan) + Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G; + else + Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G; + } + + if (b2_4GBand) { + for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) { + channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index]; + + if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */ + (RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)) { + if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11) + channel_set[chanset_size].ScanType = SCAN_ACTIVE; + else if ((channel_set[chanset_size].ChannelNum >= 12 && channel_set[chanset_size].ChannelNum <= 14)) + channel_set[chanset_size].ScanType = SCAN_PASSIVE; + } else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan || + RT_CHANNEL_DOMAIN_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 < RTW_ChannelPlan5G[Index5G].Len; index++) { + if (RTW_ChannelPlan5G[Index5G].Channel[index] <= 48 + || RTW_ChannelPlan5G[Index5G].Channel[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_871X("%s(): channel_set[%d].ChannelNum = %d\n", __func__, chanset_size, channel_set[chanset_size].ChannelNum); + chanset_size++; + } + } + } + + DBG_871X("%s ChannelPlan ID %x Chan num:%d \n", __func__, ChannelPlan, chanset_size); + return chanset_size; +} + +int init_mlme_ext_priv(struct adapter *padapter) +{ + int res = _SUCCESS; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */ + /* memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv)); */ + + pmlmeext->padapter = padapter; + + /* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */ + + init_mlme_ext_priv_value(padapter); + pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq; + + init_mlme_ext_timer(padapter); + + init_mlme_ap_info(padapter); + + 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->last_scan_time = 0; + pmlmeext->chan_scan_time = SURVEY_TO; + pmlmeext->mlmeext_init = true; + pmlmeext->active_keep_alive_check = true; + +#ifdef DBG_FIXED_CHAN + pmlmeext->fixed_chan = 0xFF; +#endif + + return res; + +} + +void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext) +{ + struct 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_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame) +{ + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 *pframe = precv_frame->u.hdr.rx_data; + + if (ptable->func) { + /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */ + if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) && + memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) + return; + + ptable->func(padapter, precv_frame); + } +} + +void mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame) +{ + int index; + struct mlme_handler *ptable; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe)); + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n", + GetFrameType(pframe), GetFrameSubType(pframe))); + + if (GetFrameType(pframe) != WIFI_MGT_TYPE) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe))); + return; + } + + /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */ + if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) && + memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) { + return; + } + + ptable = mlme_sta_tbl; + + index = GetFrameSubType(pframe) >> 4; + + if (index >= (sizeof(mlme_sta_tbl) / sizeof(struct mlme_handler))) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type =%d\n", index)); + return; + } + ptable += index; + + if (psta != NULL) { + if (GetRetry(pframe)) { + if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) { + /* drop the duplicate management frame */ + pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++; + DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num); + return; + } + } + psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num; + } + + switch (GetFrameSubType(pframe)) { + case WIFI_AUTH: + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) + ptable->func = &OnAuth; + else + ptable->func = &OnAuthClient; + /* pass through */ + case WIFI_ASSOCREQ: + case WIFI_REASSOCREQ: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + case WIFI_PROBEREQ: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + case WIFI_BEACON: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + case WIFI_ACTION: + /* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */ + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + default: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + } +} + +/**************************************************************************** + +Following are the callback functions for each subtype of the management frames + +*****************************************************************************/ + +unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ielen; + unsigned char *p; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *cur = &(pmlmeinfo->network); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + u8 is_valid_p2p_probereq = false; + + 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; + } + + + /* DBG_871X("+OnProbeReq\n"); */ + +#ifdef CONFIG_AUTO_AP_MODE + if (check_fwstate(pmlmepriv, _FW_LINKED) == true && + pmlmepriv->cur_network.join_res == true) { + struct sta_info *psta; + u8 *mac_addr, *peer_addr; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A}; + /* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */ + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + if (!p || ielen != 14) + goto _non_rc_device; + + if (memcmp(p+2, RC_OUI, sizeof(RC_OUI))) + goto _non_rc_device; + + if (memcmp(p+6, get_sa(pframe), ETH_ALEN)) { + DBG_871X("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __func__, + MAC_ARG(get_sa(pframe)), MAC_ARG(p+6)); + + goto _non_rc_device; + } + + DBG_871X("%s, got the pairing device("MAC_FMT")\n", __func__, MAC_ARG(get_sa(pframe))); + + /* new a station */ + psta = rtw_get_stainfo(pstapriv, get_sa(pframe)); + if (psta == NULL) { + /* allocate a new one */ + DBG_871X("going to alloc stainfo for rc ="MAC_FMT"\n", MAC_ARG(get_sa(pframe))); + psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe)); + if (psta == NULL) { + /* TODO: */ + DBG_871X(" Exceed the upper limit of supported clients...\n"); + return _SUCCESS; + } + + spin_lock_bh(&pstapriv->asoc_list_lock); + if (list_empty(&psta->asoc_list)) { + psta->expire_to = pstapriv->expire_to; + list_add_tail(&psta->asoc_list, &pstapriv->asoc_list); + pstapriv->asoc_list_cnt++; + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + /* generate pairing ID */ + mac_addr = myid(&(padapter->eeprompriv)); + peer_addr = psta->hwaddr; + psta->pid = (u16)(((mac_addr[4]<<8) + mac_addr[5]) + ((peer_addr[4]<<8) + peer_addr[5])); + + /* update peer stainfo */ + psta->isrc = true; + /* psta->aid = 0; */ + /* psta->mac_id = 2; */ + + /* get a unique AID */ + if (psta->aid > 0) { + DBG_871X("old AID %d\n", psta->aid); + } else { + for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++) + if (pstapriv->sta_aid[psta->aid - 1] == NULL) + break; + + if (psta->aid > pstapriv->max_num_sta) { + psta->aid = 0; + DBG_871X("no room for more AIDs\n"); + return _SUCCESS; + } else { + pstapriv->sta_aid[psta->aid - 1] = psta; + DBG_871X("allocate new AID = (%d)\n", psta->aid); + } + } + + psta->qos_option = 1; + psta->bw_mode = CHANNEL_WIDTH_20; + psta->ieee8021x_blocked = false; + psta->htpriv.ht_option = true; + psta->htpriv.ampdu_enable = false; + psta->htpriv.sgi_20m = false; + psta->htpriv.sgi_40m = false; + psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true); + + memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + spin_lock_bh(&psta->lock); + psta->state |= _FW_LINKED; + spin_unlock_bh(&psta->lock); + + report_add_sta_event(padapter, psta->hwaddr, psta->aid); + + } + + issue_probersp(padapter, get_sa(pframe), false); + + return _SUCCESS; + + } + +_non_rc_device: + + return _SUCCESS; + +#endif /* CONFIG_AUTO_AP_MODE */ + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + + /* check (wildcard) SSID */ + if (p != NULL) { + if (is_valid_p2p_probereq == true) + goto _issue_probersp; + + if ((ielen != 0 && false == !memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) + || (ielen == 0 && pmlmeinfo->hidden_ssid_mode) + ) + return _SUCCESS; + +_issue_probersp: + if (((check_fwstate(pmlmepriv, _FW_LINKED) == true && + pmlmepriv->cur_network.join_res == true)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) { + /* DBG_871X("+issue_probersp during ap mode\n"); */ + issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq); + } + + } + + return _SUCCESS; + +} + +unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + report_survey_event(padapter, precv_frame); + return _SUCCESS; + } + + return _SUCCESS; + +} + +unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame) +{ + int cam_idx; + struct sta_info *psta; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + struct wlan_bssid_ex *pbss; + int ret = _SUCCESS; + u8 *p = NULL; + u32 ielen = 0; + + p = rtw_get_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.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_871X("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe))); + *(p + 1) = ielen - 1; + } + } + + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + report_survey_event(padapter, precv_frame); + return _SUCCESS; + } + + if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) { + if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) { + /* we should update current network before auth, or some IE is wrong */ + pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex)); + if (pbss) { + if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) { + update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true); + rtw_get_bcn_info(&(pmlmepriv->cur_network)); + } + kfree((u8 *)pbss); + } + + /* check the vendor of the assoc AP */ + pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct ieee80211_hdr_3addr)); + + /* update TSF Value */ + update_TSF(pmlmeext, pframe, len); + + /* reset for adaptive_early_32k */ + pmlmeext->adaptive_tsf_done = false; + pmlmeext->DrvBcnEarly = 0xff; + pmlmeext->DrvBcnTimeOut = 0xff; + pmlmeext->bcn_cnt = 0; + memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt)); + memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio)); + + /* start auth */ + start_clnt_auth(padapter); + + return _SUCCESS; + } + + if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (psta != NULL) { + ret = rtw_check_bcn_info(padapter, pframe, len); + if (!ret) { + DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n "); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress, 0); + return _SUCCESS; + } + /* update WMM, ERP in the beacon */ + /* todo: the timer is used instead of the number of the beacon received */ + if ((sta_rx_pkts(psta) & 0xf) == 0) + /* DBG_871X("update_bcn_info\n"); */ + update_beacon_info(padapter, pframe, len, psta); + + adaptive_early_32k(pmlmeext, pframe, len); + } + } else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) { + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (psta != NULL) { + /* update WMM, ERP in the beacon */ + /* todo: the timer is used instead of the number of the beacon received */ + if ((sta_rx_pkts(psta) & 0xf) == 0) { + /* DBG_871X("update_bcn_info\n"); */ + update_beacon_info(padapter, pframe, len, psta); + } + } else{ + /* allocate a new CAM entry for IBSS station */ + cam_idx = allocate_fw_sta_entry(padapter); + if (cam_idx == NUM_STA) + goto _END_ONBEACON_; + + /* get supported rate */ + if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) { + pmlmeinfo->FW_sta_info[cam_idx].status = 0; + goto _END_ONBEACON_; + } + + /* update TSF Value */ + update_TSF(pmlmeext, pframe, len); + + /* report sta add event */ + report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx); + } + } + } + +_END_ONBEACON_: + + return _SUCCESS; + +} + +unsigned int OnAuth(struct adapter *padapter, union recv_frame *precv_frame) +{ + 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); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + u8 offset = 0; + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + DBG_871X("+OnAuth\n"); + + sa = GetAddr2Ptr(pframe); + + auth_mode = psecuritypriv->dot11AuthAlgrthm; + + if (GetPrivacy(pframe)) { + u8 *iv; + struct rx_pkt_attrib *prxattrib = &(precv_frame->u.hdr.attrib); + + prxattrib->hdrlen = WLAN_HDR_A3_LEN; + prxattrib->encrypt = _WEP40_; + + iv = pframe+prxattrib->hdrlen; + prxattrib->key_index = ((iv[3]>>6)&0x3); + + prxattrib->iv_len = 4; + prxattrib->icv_len = 4; + + rtw_wep_decrypt(padapter, (u8 *)precv_frame); + + offset = 4; + } + + algorithm = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset)); + seq = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2)); + + DBG_871X("auth alg =%x, seq =%X\n", algorithm, seq); + + if (auth_mode == 2 && + psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ && + psecuritypriv->dot11PrivacyAlgrthm != _WEP104_) + auth_mode = 0; + + if ((algorithm > 0 && auth_mode == 0) || /* rx a shared-key auth but shared not enabled */ + (algorithm == 0 && auth_mode == 1)) { /* rx a open-system auth but shared-key is enabled */ + DBG_871X("auth rejected due to bad alg [alg =%d, auth_mib =%d] %02X%02X%02X%02X%02X%02X\n", + algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]); + + status = _STATS_NO_SUPP_ALG_; + + goto auth_fail; + } + + if (rtw_access_ctrl(padapter, sa) == false) { + status = _STATS_UNABLE_HANDLE_STA_; + goto auth_fail; + } + + pstat = rtw_get_stainfo(pstapriv, sa); + if (pstat == NULL) { + + /* allocate a new one */ + DBG_871X("going to alloc stainfo for sa ="MAC_FMT"\n", MAC_ARG(sa)); + pstat = rtw_alloc_stainfo(pstapriv, sa); + if (pstat == NULL) { + DBG_871X(" Exceed the upper limit of supported clients...\n"); + status = _STATS_UNABLE_HANDLE_STA_; + goto auth_fail; + } + + pstat->state = WIFI_FW_AUTH_NULL; + pstat->auth_seq = 0; + + /* pstat->flags = 0; */ + /* pstat->capability = 0; */ + } else{ + + spin_lock_bh(&pstapriv->asoc_list_lock); + if (list_empty(&pstat->asoc_list) == false) { + 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_871X("(1)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n", + seq, pstat->auth_seq+1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + + if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3)) { + 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_871X("(2)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n", + seq, pstat->auth_seq+1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + } else{ /* shared system or auto authentication */ + if (seq == 1) { + /* prepare for the challenging txt... */ + memset((void *)pstat->chg_txt, 78, 128); + + 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_871X("checking for challenging txt...\n"); + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&ie_len, + len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4); + + if ((p == NULL) || (ie_len <= 0)) { + DBG_871X("auth rejected because challenge failure!(1)\n"); + status = _STATS_CHALLENGE_FAIL_; + goto auth_fail; + } + + if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) { + pstat->state &= (~WIFI_FW_AUTH_STATE); + pstat->state |= WIFI_FW_AUTH_SUCCESS; + /* challenging txt is correct... */ + pstat->expire_to = pstapriv->assoc_to; + } else{ + DBG_871X("auth rejected because challenge failure!\n"); + status = _STATS_CHALLENGE_FAIL_; + goto auth_fail; + } + } else{ + DBG_871X("(3)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n", + seq, pstat->auth_seq+1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + } + + + /* Now, we are going to issue_auth... */ + pstat->auth_seq = seq + 1; + + issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_)); + + if (pstat->state & WIFI_FW_AUTH_SUCCESS) + pstat->auth_seq = 0; + + + return _SUCCESS; + +auth_fail: + + if (pstat) + rtw_free_stainfo(padapter, pstat); + + pstat = &stat; + memset((char *)pstat, '\0', sizeof(stat)); + pstat->auth_seq = 2; + memcpy(pstat->hwaddr, sa, 6); + + issue_auth(padapter, pstat, (unsigned short)status); + + return _FAIL; + +} + +unsigned int OnAuthClient(struct adapter *padapter, union 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); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; + + DBG_871X("%s\n", __func__); + + /* check A1 matches or not */ + if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN)) + return _SUCCESS; + + if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE)) + return _SUCCESS; + + offset = (GetPrivacy(pframe)) ? 4 : 0; + + algthm = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset)); + seq = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2)); + status = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4)); + + if (status != 0) { + DBG_871X("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_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len, + pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_); + + if (p == NULL) { + /* DBG_871X("marc: no challenge text?\n"); */ + goto authclnt_fail; + } + + memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len); + pmlmeinfo->auth_seq = 3; + issue_auth(padapter, NULL, 0); + set_link_timer(pmlmeext, REAUTH_TO); + + return _SUCCESS; + } else{ + /* open system */ + go2asoc = 1; + } + } else if (seq == 4) { + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) { + go2asoc = 1; + } else{ + goto authclnt_fail; + } + } else{ + /* this is also illegal */ + /* DBG_871X("marc: clnt auth failed due to illegal seq =%x\n", seq); */ + goto authclnt_fail; + } + + if (go2asoc) { + DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n"); + start_clnt_assoc(padapter); + return _SUCCESS; + } + +authclnt_fail: + + /* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */ + + return _FAIL; + +} + +unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame) +{ + 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 = _STATS_SUCCESSFUL_; + unsigned short frame_type, ie_offset = 0; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *cur = &(pmlmeinfo->network); + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + frame_type = GetFrameSubType(pframe); + if (frame_type == WIFI_ASSOCREQ) { + reassoc = 0; + ie_offset = _ASOCREQ_IE_OFFSET_; + } else{ /* WIFI_REASSOCREQ */ + reassoc = 1; + ie_offset = _REASOCREQ_IE_OFFSET_; + } + + + if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) { + DBG_871X("handle_assoc(reassoc =%d) - too short payload (len =%lu)" + "\n", reassoc, (unsigned long)pkt_len); + return _FAIL; + } + + pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (pstat == (struct sta_info *)NULL) { + status = _RSON_CLS2_; + goto asoc_class2_error; + } + + capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN); + /* capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); */ + /* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2)); */ + listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2); + + left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset); + pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset); + + + DBG_871X("%s\n", __func__); + + /* check if this stat has been successfully authenticated/assocated */ + if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) { + if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) { + status = _RSON_CLS2_; + goto asoc_class2_error; + } else{ + pstat->state &= (~WIFI_FW_ASSOC_SUCCESS); + pstat->state |= WIFI_FW_ASSOC_STATE; + } + } else{ + pstat->state &= (~WIFI_FW_AUTH_SUCCESS); + pstat->state |= WIFI_FW_ASSOC_STATE; + } + + + pstat->capability = capab_info; + + /* now parse all ieee802_11 ie to point to elems */ + if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed || + !elems.ssid) { + DBG_871X("STA " MAC_FMT " sent invalid association request\n", + MAC_ARG(pstat->hwaddr)); + status = _STATS_FAILURE_; + goto OnAssocReqFail; + } + + + /* now we should check all the fields... */ + /* checking SSID */ + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len, + pkt_len - WLAN_HDR_A3_LEN - ie_offset); + if (p == NULL) { + status = _STATS_FAILURE_; + } + + if (ie_len == 0) /* broadcast ssid, however it is not allowed in assocreq */ + status = _STATS_FAILURE_; + else { + /* check if ssid match */ + if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength)) + status = _STATS_FAILURE_; + + if (ie_len != cur->Ssid.SsidLength) + status = _STATS_FAILURE_; + } + + if (_STATS_SUCCESSFUL_ != status) + goto OnAssocReqFail; + + /* check if the supported rate is ok */ + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset); + if (p == NULL) { + DBG_871X("Rx a sta assoc-req which supported rate is empty!\n"); + /* use our own rate set as statoin used */ + /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */ + /* supportRateNum = AP_BSSRATE_LEN; */ + + status = _STATS_FAILURE_; + goto OnAssocReqFail; + } else { + memcpy(supportRate, p+2, ie_len); + supportRateNum = ie_len; + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len, + pkt_len - WLAN_HDR_A3_LEN - ie_offset); + if (p != NULL) { + + if (supportRateNum <= sizeof(supportRate)) { + memcpy(supportRate+supportRateNum, p+2, ie_len); + supportRateNum += ie_len; + } + } + } + + /* todo: mask supportRate between AP & STA -> move to update raid */ + /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */ + + /* update station supportRate */ + pstat->bssratelen = supportRateNum; + memcpy(pstat->bssrateset, supportRate, supportRateNum); + UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen); + + /* check RSN/WPA/WPS */ + pstat->dot8021xalg = 0; + pstat->wpa_psk = 0; + pstat->wpa_group_cipher = 0; + pstat->wpa2_group_cipher = 0; + pstat->wpa_pairwise_cipher = 0; + pstat->wpa2_pairwise_cipher = 0; + memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie)); + if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) { + + int group_cipher = 0, pairwise_cipher = 0; + + wpa_ie = elems.rsn_ie; + wpa_ie_len = elems.rsn_ie_len; + + if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) { + pstat->dot8021xalg = 1;/* psk, todo:802.1x */ + pstat->wpa_psk |= BIT(1); + + pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher; + pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher; + + if (!pstat->wpa2_group_cipher) + status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID; + + if (!pstat->wpa2_pairwise_cipher) + status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID; + } else{ + status = WLAN_STATUS_INVALID_IE; + } + + } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) { + + int group_cipher = 0, pairwise_cipher = 0; + + wpa_ie = elems.wpa_ie; + wpa_ie_len = elems.wpa_ie_len; + + if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) { + pstat->dot8021xalg = 1;/* psk, todo:802.1x */ + pstat->wpa_psk |= BIT(0); + + pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher; + pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher; + + if (!pstat->wpa_group_cipher) + status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID; + + if (!pstat->wpa_pairwise_cipher) + status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID; + + } else{ + status = WLAN_STATUS_INVALID_IE; + } + + } else { + wpa_ie = NULL; + wpa_ie_len = 0; + } + + if (_STATS_SUCCESSFUL_ != status) + goto OnAssocReqFail; + + pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS); + if (wpa_ie == NULL) { + if (elems.wps_ie) { + DBG_871X("STA included WPS IE in " + "(Re)Association Request - assume WPS is " + "used\n"); + pstat->flags |= WLAN_STA_WPS; + /* wpabuf_free(sta->wps_ie); */ + /* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */ + /* elems.wps_ie_len - 4); */ + } else { + DBG_871X("STA did not include WPA/RSN IE " + "in (Re)Association Request - possible WPS " + "use\n"); + pstat->flags |= WLAN_STA_MAYBE_WPS; + } + + + /* AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */ + /* that the selected registrar of AP is _FLASE */ + if ((psecuritypriv->wpa_psk > 0) + && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) { + if (pmlmepriv->wps_beacon_ie) { + u8 selected_registrar = 0; + + rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL); + + if (!selected_registrar) { + DBG_871X("selected_registrar is false , or AP is not ready to do WPS\n"); + + status = _STATS_UNABLE_HANDLE_STA_; + + goto OnAssocReqFail; + } + } + } + + } else{ + int copy_len; + + if (psecuritypriv->wpa_psk == 0) { + DBG_871X("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 OnAssocReqFail; + + } + + if (elems.wps_ie) { + DBG_871X("STA included WPS IE in " + "(Re)Association Request - WPS is " + "used\n"); + pstat->flags |= WLAN_STA_WPS; + copy_len = 0; + } else{ + copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2); + } + + + if (copy_len > 0) + memcpy(pstat->wpa_ie, wpa_ie-2, copy_len); + + } + + + /* check if there is WMM IE & support WWM-PS */ + pstat->flags &= ~WLAN_STA_WME; + pstat->qos_option = 0; + pstat->qos_info = 0; + pstat->has_legacy_ac = true; + pstat->uapsd_vo = 0; + pstat->uapsd_vi = 0; + pstat->uapsd_be = 0; + pstat->uapsd_bk = 0; + if (pmlmepriv->qospriv.qos_option) { + p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0; + for (;;) { + p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset); + if (p != NULL) { + if (!memcmp(p+2, WMM_IE, 6)) { + + pstat->flags |= WLAN_STA_WME; + + pstat->qos_option = 1; + pstat->qos_info = *(p+8); + + pstat->max_sp_len = (pstat->qos_info>>5)&0x3; + + if ((pstat->qos_info&0xf) != 0xf) + pstat->has_legacy_ac = true; + else + pstat->has_legacy_ac = false; + + if (pstat->qos_info&0xf) { + if (pstat->qos_info&BIT(0)) + pstat->uapsd_vo = BIT(0)|BIT(1); + else + pstat->uapsd_vo = 0; + + if (pstat->qos_info&BIT(1)) + pstat->uapsd_vi = BIT(0)|BIT(1); + else + pstat->uapsd_vi = 0; + + if (pstat->qos_info&BIT(2)) + pstat->uapsd_bk = BIT(0)|BIT(1); + else + pstat->uapsd_bk = 0; + + if (pstat->qos_info&BIT(3)) + pstat->uapsd_be = BIT(0)|BIT(1); + else + pstat->uapsd_be = 0; + + } + + break; + } + } else { + break; + } + p = p + ie_len + 2; + } + } + + /* save HT capabilities in the sta object */ + memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap)); + if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) { + pstat->flags |= WLAN_STA_HT; + + pstat->flags |= WLAN_STA_WME; + + memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap)); + + } else + pstat->flags &= ~WLAN_STA_HT; + + + if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT)) { + status = _STATS_FAILURE_; + goto OnAssocReqFail; + } + + + if ((pstat->flags & WLAN_STA_HT) && + ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) || + (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) { + DBG_871X("HT: " MAC_FMT " tried to " + "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr)); + + /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */ + /* goto OnAssocReqFail; */ + } + pstat->flags |= WLAN_STA_NONERP; + for (i = 0; i < pstat->bssratelen; i++) { + if ((pstat->bssrateset[i] & 0x7f) > 22) { + pstat->flags &= ~WLAN_STA_NONERP; + break; + } + } + + if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) + pstat->flags |= WLAN_STA_SHORT_PREAMBLE; + else + pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE; + + + + if (status != _STATS_SUCCESSFUL_) + goto OnAssocReqFail; + + /* 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_871X(" old AID %d\n", pstat->aid); + } else { + for (pstat->aid = 1; pstat->aid < NUM_STA; pstat->aid++) + if (pstapriv->sta_aid[pstat->aid - 1] == NULL) + break; + + /* if (pstat->aid > NUM_STA) { */ + if (pstat->aid > pstapriv->max_num_sta) { + + pstat->aid = 0; + + DBG_871X(" no room for more AIDs\n"); + + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + + goto OnAssocReqFail; + + + } else { + pstapriv->sta_aid[pstat->aid - 1] = pstat; + DBG_871X("allocate new AID = (%d)\n", pstat->aid); + } + } + + + pstat->state &= (~WIFI_FW_ASSOC_STATE); + pstat->state |= WIFI_FW_ASSOC_SUCCESS; + + spin_lock_bh(&pstapriv->auth_list_lock); + if (!list_empty(&pstat->auth_list)) { + list_del_init(&pstat->auth_list); + pstapriv->auth_list_cnt--; + } + spin_unlock_bh(&pstapriv->auth_list_lock); + + spin_lock_bh(&pstapriv->asoc_list_lock); + if (list_empty(&pstat->asoc_list)) { + pstat->expire_to = pstapriv->expire_to; + list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list); + pstapriv->asoc_list_cnt++; + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + /* now the station is qualified to join our BSS... */ + if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) { + /* 1 bss_cap_update & sta_info_update */ + bss_cap_update_on_sta_join(padapter, pstat); + sta_info_update(padapter, pstat); + + /* 2 issue assoc rsp before notify station join event. */ + if (frame_type == WIFI_ASSOCREQ) + issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP); + + spin_lock_bh(&pstat->lock); + if (pstat->passoc_req) { + kfree(pstat->passoc_req); + pstat->passoc_req = NULL; + pstat->assoc_req_len = 0; + } + + pstat->passoc_req = rtw_zmalloc(pkt_len); + if (pstat->passoc_req) { + memcpy(pstat->passoc_req, pframe, pkt_len); + pstat->assoc_req_len = pkt_len; + } + spin_unlock_bh(&pstat->lock); + + /* 3-(1) report sta add event */ + report_add_sta_event(padapter, pstat->hwaddr, pstat->aid); + } + + return _SUCCESS; + +asoc_class2_error: + + issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status); + + return _FAIL; + +OnAssocReqFail: + + pstat->aid = 0; + if (frame_type == WIFI_ASSOCREQ) + issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP); + + return _FAIL; +} + +unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame) +{ + uint i; + int res; + unsigned short status; + struct ndis_80211_var_ie *pIE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + /* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */ + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; + + DBG_871X("%s\n", __func__); + + /* check A1 matches or not */ + if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN)) + return _SUCCESS; + + if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE))) + return _SUCCESS; + + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + return _SUCCESS; + + del_timer_sync(&pmlmeext->link_timer); + + /* status */ + status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2)); + if (status > 0) { + DBG_871X("assoc reject, status code: %d\n", status); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + res = -4; + goto report_assoc_result; + } + + /* get capabilities */ + pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN)); + + /* set slot time */ + pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20; + + /* AID */ + res = pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 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 + WLAN_HDR_A3_LEN); i < pkt_len;) { + pIE = (struct ndis_80211_var_ie *)(pframe + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (!memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */ + WMM_param_handler(padapter, pIE); + break; + + case _HT_CAPABILITY_IE_: /* HT caps */ + HT_caps_handler(padapter, pIE); + break; + + case _HT_EXTRA_INFO_IE_: /* HT info */ + HT_info_handler(padapter, pIE); + break; + + case _ERPINFO_IE_: + ERP_IE_handler(padapter, pIE); + + default: + break; + } + + i += (pIE->Length + 2); + } + + pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + + /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */ + UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates); + +report_assoc_result: + if (res > 0) { + rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len); + } else { + rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len); + } + + report_join_res(padapter, res); + + return _SUCCESS; +} + +unsigned int OnDeAuth(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned short reason; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + + /* check A3 */ + if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) + return _SUCCESS; + + reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN)); + + DBG_871X("%s Reason code(%d)\n", __func__, reason); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* spin_lock_bh(&(pstapriv->sta_hash_lock)); */ + /* rtw_free_stainfo(padapter, psta); */ + /* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */ + + DBG_871X_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n", + reason, GetAddr2Ptr(pframe)); + + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (psta) { + u8 updated = false; + + spin_lock_bh(&pstapriv->asoc_list_lock); + if (list_empty(&psta->asoc_list) == false) { + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + updated = ap_free_sta(padapter, psta, false, reason); + + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + associated_clients_update(padapter, updated); + } + + + return _SUCCESS; + } else{ + int ignore_received_deauth = 0; + + /* Commented by Albert 20130604 */ + /* Before sending the auth frame to start the STA/GC mode connection with AP/GO, */ + /* we will send the deauth first. */ + /* However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth. */ + /* Added the following code to avoid this case. */ + if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) || + (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)) { + if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA) { + ignore_received_deauth = 1; + } else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) { + /* TODO: 802.11r */ + ignore_received_deauth = 1; + } + } + + DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n", + reason, GetAddr3Ptr(pframe), ignore_received_deauth); + + if (0 == ignore_received_deauth) { + receive_disconnect(padapter, GetAddr3Ptr(pframe), reason); + } + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = false; + return _SUCCESS; + +} + +unsigned int OnDisassoc(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned short reason; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + + /* check A3 */ + if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) + return _SUCCESS; + + reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN)); + + DBG_871X("%s Reason code(%d)\n", __func__, reason); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* spin_lock_bh(&(pstapriv->sta_hash_lock)); */ + /* rtw_free_stainfo(padapter, psta); */ + /* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */ + + DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n", + reason, GetAddr2Ptr(pframe)); + + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (psta) { + u8 updated = false; + + spin_lock_bh(&pstapriv->asoc_list_lock); + if (list_empty(&psta->asoc_list) == false) { + list_del_init(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + updated = ap_free_sta(padapter, psta, false, reason); + + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + associated_clients_update(padapter, updated); + } + + return _SUCCESS; + } else{ + DBG_871X_LEVEL(_drv_always_, "sta recv disassoc reason code(%d) sta:%pM\n", + reason, GetAddr3Ptr(pframe)); + + receive_disconnect(padapter, GetAddr3Ptr(pframe), reason); + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = false; + return _SUCCESS; + +} + +unsigned int OnAtim(struct adapter *padapter, union recv_frame *precv_frame) +{ + DBG_871X("%s\n", __func__); + return _SUCCESS; +} + +unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = (u8 *)(pframe + sizeof(struct ieee80211_hdr_3addr)); + u8 category; + u8 action; + + DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev)); + + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + + if (!psta) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_WLAN_ACTION_SPCT_MSR_REQ: + case RTW_WLAN_ACTION_SPCT_MSR_RPRT: + case RTW_WLAN_ACTION_SPCT_TPC_REQ: + case RTW_WLAN_ACTION_SPCT_TPC_RPRT: + case RTW_WLAN_ACTION_SPCT_CHL_SWITCH: + break; + default: + break; + } + +exit: + return ret; +} + +unsigned int OnAction_back(struct adapter *padapter, union recv_frame *precv_frame) +{ + u8 *addr; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + unsigned char *frame_body; + unsigned char category, action; + unsigned short tid, status, reason_code = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_871X("%s\n", __func__); + + /* check RA matches or not */ + if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */ + return _SUCCESS; + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) + return _SUCCESS; + + addr = GetAddr2Ptr(pframe); + psta = rtw_get_stainfo(pstapriv, addr); + + if (psta == NULL) + return _SUCCESS; + + frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr)); + + category = frame_body[0]; + if (category == RTW_WLAN_CATEGORY_BACK) {/* representing Block Ack */ + if (!pmlmeinfo->HT_enable) { + return _SUCCESS; + } + + action = frame_body[1]; + DBG_871X("%s, action =%d\n", __func__, action); + switch (action) { + case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */ + + memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request)); + /* process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe)); */ + process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr); + + if (pmlmeinfo->bAcceptAddbaReq == true) { + issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0); + } else{ + issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */ + } + + break; + + case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */ + status = RTW_GET_LE16(&frame_body[3]); + tid = ((frame_body[5] >> 2) & 0x7); + + if (status == 0) { + /* successful */ + DBG_871X("agg_enable for TID =%d\n", tid); + psta->htpriv.agg_enable_bitmap |= 1 << tid; + psta->htpriv.candidate_tid_bitmap &= ~BIT(tid); + } else{ + psta->htpriv.agg_enable_bitmap &= ~BIT(tid); + } + + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) { + DBG_871X("%s alive check - rx ADDBA response\n", __func__); + psta->htpriv.agg_enable_bitmap &= ~BIT(tid); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + + /* DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap); */ + break; + + case RTW_WLAN_ACTION_DELBA: /* DELBA */ + if ((frame_body[3] & BIT(3)) == 0) { + psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf)); + psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf)); + + /* reason_code = 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; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__, + preorder_ctrl->indicate_seq); + #endif + } + + DBG_871X("%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; +} + +static s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token) +{ + struct adapter *adapter = recv_frame->u.hdr.adapter; + struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv); + u8 *frame = recv_frame->u.hdr.rx_data; + u16 seq_ctrl = ((recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) | + (recv_frame->u.hdr.attrib.frag_num & 0xf); + + if (GetRetry(frame)) { + if (token >= 0) { + if ((seq_ctrl == mlmeext->action_public_rxseq) + && (token == mlmeext->action_public_dialog_token)) { + DBG_871X(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_871X(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n", + FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq); + return _FAIL; + } + } + } + + mlmeext->action_public_rxseq = seq_ctrl; + + if (token >= 0) + mlmeext->action_public_dialog_token = token; + + return _SUCCESS; +} + +static unsigned int on_action_public_p2p(union recv_frame *precv_frame) +{ + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body; + u8 dialogToken = 0; + + 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; + + return _SUCCESS; +} + +static unsigned int on_action_public_vendor(union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + + if (!memcmp(frame_body + 2, P2P_OUI, 4)) { + ret = on_action_public_p2p(precv_frame); + } + + return ret; +} + +static unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint frame_len = precv_frame->u.hdr.len; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + u8 token; + struct adapter *adapter = precv_frame->u.hdr.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_str(action), token); + rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg); + + ret = _SUCCESS; + +exit: + return ret; +} + +unsigned int on_action_public(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + u8 category, action; + + /* check RA matches or not */ + if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_PUBLIC) + goto exit; + + action = frame_body[1]; + switch (action) { + case ACT_PUBLIC_VENDOR: + ret = on_action_public_vendor(precv_frame); + break; + default: + ret = on_action_public_default(precv_frame, action); + break; + } + +exit: + return ret; +} + +unsigned int OnAction_ht(struct adapter *padapter, union recv_frame *precv_frame) +{ + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr); + u8 category, action; + + /* check RA matches or not */ + if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_HT) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING: + break; + default: + break; + } + +exit: + + return _SUCCESS; +} + +unsigned int OnAction_sa_query(struct adapter *padapter, union recv_frame *precv_frame) +{ + u8 *pframe = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + unsigned short tid; + /* Baron */ + + DBG_871X("OnAction_sa_query\n"); + + switch (pframe[WLAN_HDR_A3_LEN+1]) { + case 0: /* SA Query req */ + memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short)); + DBG_871X("OnAction_sa_query request, action =%d, tid =%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid); + issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid); + break; + + case 1: /* SA Query rsp */ + del_timer_sync(&pmlmeext->sa_query_timer); + DBG_871X("OnAction_sa_query response, action =%d, tid =%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN+1], pframe[WLAN_HDR_A3_LEN+2]); + break; + default: + break; + } + if (0) { + int pp; + printk("pattrib->pktlen = %d =>", pattrib->pkt_len); + for (pp = 0; pp < pattrib->pkt_len; pp++) + printk(" %02x ", pframe[pp]); + printk("\n"); + } + + return _SUCCESS; +} + +unsigned int OnAction(struct adapter *padapter, union recv_frame *precv_frame) +{ + int i; + unsigned char category; + struct action_handler *ptable; + unsigned char *frame_body; + u8 *pframe = precv_frame->u.hdr.rx_data; + + frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr)); + + category = frame_body[0]; + + for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) { + ptable = &OnAction_tbl[i]; + + if (category == ptable->num) + ptable->func(padapter, precv_frame); + + } + + return _SUCCESS; + +} + +unsigned int DoReserved(struct adapter *padapter, union recv_frame *precv_frame) +{ + + /* DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe)); */ + return _SUCCESS; +} + +static struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once) +{ + struct xmit_frame *pmgntframe; + struct xmit_buf *pxmitbuf; + + if (once) + pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv); + else + pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv); + + if (pmgntframe == NULL) { + DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once); + goto exit; + } + + pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv); + if (pxmitbuf == NULL) { + DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter)); + rtw_free_xmitframe(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; + +} + +inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv) +{ + return _alloc_mgtxmitframe(pxmitpriv, false); +} + +/**************************************************************************** + +Following are some TX fuctions for WiFi MLME + +*****************************************************************************/ + +void update_mgnt_tx_rate(struct adapter *padapter, u8 rate) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + pmlmeext->tx_rate = rate; + /* DBG_871X("%s(): rate = %x\n", __func__, rate); */ +} + +void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib) +{ + u8 wireless_mode; + 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->tx_rate == IEEE80211_CCK_RATE_1MB) + wireless_mode = WIRELESS_11B; + else + wireless_mode = WIRELESS_11G; + pattrib->raid = rtw_get_mgntframe_raid(padapter, wireless_mode); + pattrib->rate = pmlmeext->tx_rate; + + pattrib->encrypt = _NO_PRIVACY_; + pattrib->bswenc = false; + + pattrib->qos_en = false; + pattrib->ht_en = false; + pattrib->bwmode = CHANNEL_WIDTH_20; + pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pattrib->sgi = false; + + pattrib->seqnum = pmlmeext->mgnt_seq; + + pattrib->retry_ctrl = true; + + pattrib->mbssid = 0; + +} + +void update_mgntframe_attrib_addr(struct adapter *padapter, struct xmit_frame *pmgntframe) +{ + u8 *pframe; + struct pkt_attrib *pattrib = &pmgntframe->attrib; + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + + memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN); + memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN); +} + +void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe) +{ + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return; + } + + rtw_hal_mgnt_xmit(padapter, pmgntframe); +} + +s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms) +{ + s32 ret = _FAIL; + _irqL irqL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf; + struct submit_ctx sctx; + + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return ret; + } + + rtw_sctx_init(&sctx, timeout_ms); + pxmitbuf->sctx = &sctx; + + ret = rtw_hal_mgnt_xmit(padapter, pmgntframe); + + if (ret == _SUCCESS) + ret = rtw_sctx_wait(&sctx, __func__); + + spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL); + pxmitbuf->sctx = NULL; + spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL); + + return ret; +} + +s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe) +{ + static u8 seq_no = 0; + s32 ret = _FAIL; + u32 timeout_ms = 500;/* 500ms */ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter->bSurpriseRemoved == true || + padapter->bDriverStopped == true) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return -1; + } + + if (mutex_lock_interruptible(&pxmitpriv->ack_tx_mutex) == 0) { + pxmitpriv->ack_tx = true; + pxmitpriv->seq_no = seq_no++; + pmgntframe->ack_report = 1; + if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) { + ret = rtw_ack_tx_wait(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; + sint ssid_len_ori; + int len_diff = 0; + + ssid_ie = rtw_get_ie(ies, WLAN_EID_SSID, &ssid_len_ori, ies_len); + + /* DBG_871X("%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: + { + u8 *next_ie = ssid_ie + 2 + ssid_len_ori; + u32 remain_len = 0; + + remain_len = ies_len - (next_ie-ies); + + ssid_ie[1] = 0; + memcpy(ssid_ie+2, next_ie, remain_len); + len_diff -= ssid_len_ori; + + break; + } + case 2: + memset(&ssid_ie[2], 0, ssid_len_ori); + break; + default: + break; + } + } + + return len_diff; +} + +void issue_beacon(struct adapter *padapter, int timeout_ms) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + unsigned int rate_len; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + /* DBG_871X("%s\n", __func__); */ + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + DBG_871X("%s, alloc mgnt frame fail\n", __func__); + return; + } + + spin_lock_bh(&pmlmepriv->bcn_update_lock); + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->qsel = 0x10; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN); + + SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/); + /* pmlmeext->mgnt_seq++; */ + SetFrameSubType(pframe, WIFI_BEACON); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) { + /* DBG_871X("ie len =%d\n", cur_network->IELength); */ + { + int len_diff; + memcpy(pframe, cur_network->IEs, cur_network->IELength); + len_diff = update_hidden_ssid( + pframe+_BEACON_IE_OFFSET_ + , cur_network->IELength-_BEACON_IE_OFFSET_ + , pmlmeinfo->hidden_ssid_mode + ); + pframe += (cur_network->IELength+len_diff); + pattrib->pktlen += (cur_network->IELength+len_diff); + } + + { + u8 *wps_ie; + uint wps_ielen; + u8 sr = 0; + wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct 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_content(wps_ie, wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL); + } + if (sr != 0) + set_fwstate(pmlmepriv, WIFI_UNDER_WPS); + else + _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS); + } + + goto _issue_bcn; + + } + + /* below for ad-hoc mode */ + + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + + memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len(cur_network->SupportedRates); + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen); + + /* if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */ + { + u8 erpinfo = 0; + u32 ATIMWindow; + /* IBSS Parameter Set... */ + /* ATIMWindow = cur->Configuration.ATIMWindow; */ + ATIMWindow = 0; + pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen); + } + + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) { + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen); + } + + + /* todo:HT for adhoc */ + +_issue_bcn: + + pmlmepriv->update_bcn = false; + + spin_unlock_bh(&pmlmepriv->bcn_update_lock); + + if ((pattrib->pktlen + TXDESC_SIZE) > 512) { + DBG_871X("beacon frame too large\n"); + return; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + /* DBG_871X("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */ + if (timeout_ms > 0) + dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms); + else + dump_mgntframe(padapter, pmgntframe); + +} + +void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + unsigned char *mac, *bssid; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + + u8 *pwps_ie; + uint wps_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 *cur_network = &(pmlmeinfo->network); + unsigned int rate_len; + + /* DBG_871X("%s\n", __func__); */ + + if (da == NULL) + return; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + DBG_871X("%s, alloc mgnt frame fail\n", __func__); + return; + } + + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&(padapter->eeprompriv)); + bssid = cur_network->MacAddress; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + memcpy(pwlanhdr->addr3, bssid, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(fctrl, WIFI_PROBERSP); + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = pattrib->hdrlen; + pframe += pattrib->hdrlen; + + + if (cur_network->IELength > MAX_IE_SZ) + return; + + if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) { + pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen); + + /* inerset & update wps_probe_resp_ie */ + if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie && (wps_ielen > 0)) { + uint wps_offset, remainder_ielen; + u8 *premainder_ie; + + wps_offset = (uint)(pwps_ie - cur_network->IEs); + + premainder_ie = pwps_ie + wps_ielen; + + remainder_ielen = cur_network->IELength - wps_offset - wps_ielen; + + memcpy(pframe, cur_network->IEs, wps_offset); + pframe += wps_offset; + pattrib->pktlen += wps_offset; + + wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */ + if ((wps_offset+wps_ielen+2) <= MAX_IE_SZ) { + memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2); + pframe += wps_ielen+2; + pattrib->pktlen += wps_ielen+2; + } + + if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) { + memcpy(pframe, premainder_ie, remainder_ielen); + pframe += remainder_ielen; + pattrib->pktlen += remainder_ielen; + } + } else{ + memcpy(pframe, cur_network->IEs, cur_network->IELength); + pframe += cur_network->IELength; + pattrib->pktlen += cur_network->IELength; + } + + /* retrieve SSID IE from cur_network->Ssid */ + { + u8 *ssid_ie; + sint ssid_ielen; + sint ssid_ielen_diff; + u8 buf[MAX_IE_SZ]; + u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct ieee80211_hdr_3addr); + + ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen, + (pframe-ies)-_FIXED_IE_LENGTH_); + + ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen; + + if (ssid_ie && cur_network->Ssid.SsidLength) { + uint remainder_ielen; + u8 *remainder_ie; + remainder_ie = ssid_ie+2; + remainder_ielen = (pframe-remainder_ie); + + if (remainder_ielen > MAX_IE_SZ) { + DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter)); + 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.SsidLength; + memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength); + + pframe += ssid_ielen_diff; + pattrib->pktlen += ssid_ielen_diff; + } + } + } else{ + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + + memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* below for ad-hoc mode */ + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len(cur_network->SupportedRates); + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) { + u8 erpinfo = 0; + u32 ATIMWindow; + /* IBSS Parameter Set... */ + /* ATIMWindow = cur->Configuration.ATIMWindow; */ + ATIMWindow = 0; + pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen); + } + + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) { + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen); + } + + + /* todo:HT for adhoc */ + + } + +#ifdef CONFIG_AUTO_AP_MODE +{ + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_871X("(%s)\n", __func__); + + /* check rc station */ + psta = rtw_get_stainfo(pstapriv, da); + if (psta && psta->isrc && psta->pid > 0) { + u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A}; + u8 RC_INFO[14] = {0}; + /* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */ + u16 cu_ch = (u16)cur_network->Configuration.DSConfig; + + DBG_871X("%s, reply rc(pid = 0x%x) device "MAC_FMT" in ch =%d\n", __func__, + psta->pid, MAC_ARG(psta->hwaddr), cu_ch); + + /* append vendor specific ie */ + memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI)); + memcpy(&RC_INFO[4], mac, ETH_ALEN); + memcpy(&RC_INFO[10], (u8 *)&psta->pid, 2); + memcpy(&RC_INFO[12], (u8 *)&cu_ch, 2); + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen); + } +} +#endif /* CONFIG_AUTO_AP_MODE */ + + + pattrib->last_txcmdsz = pattrib->pktlen; + + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, u8 ch, bool append_wps, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + unsigned char *mac; + unsigned char bssrate[NumRates]; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + int bssrate_len = 0; + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n")); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + mac = myid(&(padapter->eeprompriv)); + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + if (da) { + /* unicast probe request frame */ + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr3, da, ETH_ALEN); + } else{ + /* broadcast probe request frame */ + memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN); + } + + memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_PROBEREQ); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + if (pssid) + pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen)); + else + pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen)); + + get_rate_set(padapter, bssrate, &bssrate_len); + + if (bssrate_len > 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen)); + } else{ + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &(pattrib->pktlen)); + } + + if (ch) + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, &ch, &pattrib->pktlen); + + if (append_wps) { + /* add wps_ie for wps2.0 */ + if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) { + memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len); + pframe += pmlmepriv->wps_probe_req_ie_len; + pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len; + } + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz)); + + if (wait_ack) { + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + } else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da) +{ + _issue_probereq(padapter, pssid, da, 0, 1, false); +} + +int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, u8 ch, bool append_wps, + int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + + do { + ret = _issue_probereq(padapter, pssid, da, ch, append_wps, 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; + #ifndef DBG_XMIT_ACK + goto exit; + #endif + } + + if (try_cnt && wait_ms) { + if (da) + DBG_871X(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_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + else + DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + } +exit: + return ret; +} + +/* if psta == NULL, indiate we are station(client) now... */ +void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *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); + __le16 le_tmp; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct 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 */ + memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN); + + /* setting auth algo number */ + val16 = (u16)psta->authalg; + + if (status != _STATS_SUCCESSFUL_) + val16 = 0; + + if (val16) + use_shared_key = 1; + + le_tmp = cpu_to_le16(val16); + + pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + /* setting auth seq number */ + val16 = (u16)psta->auth_seq; + le_tmp = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + /* setting status code... */ + val16 = status; + le_tmp = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + /* added challenging text... */ + if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) + pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen)); + + } else{ + memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + + /* setting auth algo number */ + val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/* 0:OPEN System, 1:Shared key */ + if (val16) { + use_shared_key = 1; + } + le_tmp = cpu_to_le16(val16); + /* DBG_871X("%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)) { + __le32 le_tmp32; + + /* DBG_871X("==> iv(%d), key_index(%d)\n", pmlmeinfo->iv, pmlmeinfo->key_index); */ + val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30)); + le_tmp32 = cpu_to_le32(val32); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen)); + + pattrib->iv_len = 4; + } + + pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + /* setting auth seq number */ + le_tmp = cpu_to_le16(pmlmeinfo->auth_seq); + pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + + /* setting status code... */ + le_tmp = cpu_to_le16(status); + pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + /* then checking to see if sending challenging text... */ + if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) { + pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen)); + + SetPrivacy(fctrl); + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + + pattrib->encrypt = _WEP40_; + + pattrib->icv_len = 4; + + pattrib->pktlen += pattrib->icv_len; + + } + + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + rtw_wep_encrypt(padapter, (u8 *)pmgntframe); + DBG_871X("%s\n", __func__); + dump_mgntframe(padapter, pmgntframe); + + return; +} + + +void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type) +{ + struct xmit_frame *pmgntframe; + struct ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + unsigned char *pbuf, *pframe; + unsigned short val; + __le16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network); + u8 *ie = pnetwork->IEs; + __le16 lestatus, le_tmp; + + DBG_871X("%s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN); + memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP)) + SetFrameSubType(pwlanhdr, pkt_type); + else + return; + + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen += pattrib->hdrlen; + pframe += pattrib->hdrlen; + + /* capability */ + val = *(unsigned short *)rtw_get_capability_from_ie(ie); + + pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_, (unsigned char *)&val, &(pattrib->pktlen)); + + lestatus = cpu_to_le16(status); + pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&lestatus, &(pattrib->pktlen)); + + le_tmp = cpu_to_le16(pstat->aid | BIT(14) | BIT(15)); + pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + if (pstat->bssratelen <= 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen)); + } else{ + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen)); + } + + if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) { + uint ie_len = 0; + + /* FILL HT CAP INFO IE */ + /* p = hostapd_eid_ht_capabilities_info(hapd, p); */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + memcpy(pframe, pbuf, ie_len+2); + pframe += (ie_len+2); + pattrib->pktlen += (ie_len+2); + } + + /* FILL HT ADD INFO IE */ + /* p = hostapd_eid_ht_operation(hapd, p); */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + memcpy(pframe, pbuf, ie_len+2); + pframe += (ie_len+2); + pattrib->pktlen += (ie_len+2); + } + + } + + /* FILL WMM IE */ + if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) { + uint ie_len = 0; + unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + + for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) { + pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2))); + if (pbuf && !memcmp(pbuf+2, WMM_PARA_IE, 6)) { + memcpy(pframe, pbuf, ie_len+2); + pframe += (ie_len+2); + pattrib->pktlen += (ie_len+2); + + break; + } + + if ((pbuf == NULL) || (ie_len == 0)) { + break; + } + } + + } + + + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) { + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6, REALTEK_96B_IE, &(pattrib->pktlen)); + } + + /* add WPS IE ie for wps 2.0 */ + if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) { + memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len); + + pframe += pmlmepriv->wps_assoc_resp_ie_len; + pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +} + +void issue_assocreq(struct adapter *padapter) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + __le16 val16; + unsigned int i, j, index = 0; + unsigned char bssrate[NumRates], sta_bssrate[NumRates]; + struct ndis_80211_var_ie *pIE; + 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; + u8 vs_ie_length = 0; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ASSOCREQ); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* caps */ + memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* listen interval */ + /* todo: listen interval for power saving */ + val16 = cpu_to_le16(3); + memcpy(pframe, (unsigned char *)&val16, 2); + pframe += 2; + pattrib->pktlen += 2; + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen)); + + /* supported rate & extended supported rate */ + + /* Check if the AP's supported rates are also supported by STA. */ + get_rate_set(padapter, sta_bssrate, &sta_bssrate_len); + /* DBG_871X("sta_bssrate_len =%d\n", sta_bssrate_len); */ + + if (pmlmeext->cur_channel == 14) /* for JAPAN, channel 14 can only uses B Mode(CCK) */ + sta_bssrate_len = 4; + + + /* for (i = 0; i < sta_bssrate_len; i++) { */ + /* DBG_871X("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_871X("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_871X("match i = %d, j =%d\n", i, j); */ + break; + } else { + /* DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */ + } + } + + if (j == sta_bssrate_len) { + /* the rate is not supported by STA */ + DBG_871X("%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_871X("bssrate_len = %d\n", bssrate_len); + + if (bssrate_len == 0) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; /* don't connect to AP if no joint supported rate */ + } + + + if (bssrate_len > 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen)); + } else + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &(pattrib->pktlen)); + + /* vendor specific IE, such as WPA, WMM, WPS */ + for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.IELength;) { + pIE = (struct ndis_80211_var_ie *)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) || + (!memcmp(pIE->data, WMM_OUI, 4)) || + (!memcmp(pIE->data, WPS_OUI, 4))) { + vs_ie_length = pIE->Length; + if ((!padapter->registrypriv.wifi_spec) && (!memcmp(pIE->data, WPS_OUI, 4))) { + /* Commented by Kurt 20110629 */ + /* In some older APs, WPS handshake */ + /* would be fail if we append vender extensions informations to AP */ + + vs_ie_length = 14; + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen)); + } + break; + + case EID_WPA2: + pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen)); + break; + case EID_HTCapability: + if (padapter->mlmepriv.htpriv.ht_option == true) { + if (!(is_ap_in_tkip(padapter))) { + memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element)); + pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length, (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen)); + } + } + break; + + case EID_EXTCapability: + if (padapter->mlmepriv.htpriv.ht_option == true) + pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen)); + break; + default: + break; + } + + i += (pIE->Length + 2); + } + + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6, REALTEK_96B_IE, &(pattrib->pktlen)); + + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); + + ret = _SUCCESS; + +exit: + if (ret == _SUCCESS) + rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen); + else + rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len); + + return; +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + + /* DBG_871X("%s:%d\n", __func__, power_mode); */ + + if (!padapter) + goto exit; + + pxmitpriv = &(padapter->xmitpriv); + pmlmeext = &(padapter->mlmeextpriv); + pmlmeinfo = &(pmlmeext->mlmext_info); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->retry_ctrl = false; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct 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); + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_DATA_NULL); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) { + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + } else{ + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +/* + * [IMPORTANT] Don't call this function in interrupt context + * + * When wait_ms > 0, this function shoule be called at process context + * da == NULL for station mode + */ +int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_info *psta; + + + /* da == NULL, assum it's null data for sta to ap*/ + if (da == NULL) + da = get_my_bssid(&(pmlmeinfo->network)); + + psta = rtw_get_stainfo(&padapter->stapriv, da); + if (psta) { + if (power_mode) + rtw_hal_macid_sleep(padapter, psta->mac_id); + else + rtw_hal_macid_wakeup(padapter, psta->mac_id); + } else { + DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup"); + rtw_warn_on(1); + } + + do { + ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0?true:false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; + #ifndef DBG_XMIT_ACK + goto exit; + #endif + } + + if (try_cnt && wait_ms) { + if (da) + DBG_871X(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_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + else + DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + } +exit: + return ret; +} + +/* + * [IMPORTANT] This function run in interrupt context + * + * The null data packet would be sent without power bit, + * and not guarantee success. + */ +s32 issue_nulldata_in_interrupt(struct adapter *padapter, u8 *da) +{ + int ret; + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + + + pmlmeext = &padapter->mlmeextpriv; + pmlmeinfo = &pmlmeext->mlmext_info; + + /* da == NULL, assum it's null data for sta to ap*/ + if (da == NULL) + da = get_my_bssid(&(pmlmeinfo->network)); + + ret = _issue_nulldata(padapter, da, 0, false); + + return ret; +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + u16 *qc; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + DBG_871X("%s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + pattrib->hdrlen += 2; + pattrib->qos_en = true; + pattrib->eosp = 1; + pattrib->ack_policy = 0; + pattrib->mdata = 0; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct 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); + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_QOS_DATA_NULL); + + pframe += sizeof(struct ieee80211_qos_hdr); + pattrib->pktlen = sizeof(struct ieee80211_qos_hdr); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) { + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + } else{ + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +/* when wait_ms >0 , this function shoule be called at process context */ +/* da == NULL for station mode */ +int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* da == NULL, assum it's null data for sta to ap*/ + if (da == NULL) + da = get_my_bssid(&(pmlmeinfo->network)); + + do { + ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0?true:false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; + #ifndef DBG_XMIT_ACK + goto exit; + #endif + } + + if (try_cnt && wait_ms) { + if (da) + DBG_871X(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_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + else + DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + } +exit: + return ret; +} + +static int _issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + int ret = _FAIL; + __le16 le_tmp; + + /* DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */ + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + goto exit; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->retry_ctrl = false; + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_DEAUTH); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + le_tmp = cpu_to_le16(reason); + pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + + if (wait_ack) { + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + } else{ + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason) +{ + DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); + return _issue_deauth(padapter, da, reason, false); +} + +int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt, + int wait_ms) +{ + int ret; + int i = 0; + + do { + ret = _issue_deauth(padapter, da, reason, wait_ms > 0?true:false); + + i++; + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + msleep(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; + #ifndef DBG_XMIT_ACK + goto exit; + #endif + } + + if (try_cnt && wait_ms) { + if (da) + DBG_871X(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_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + else + DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms); + } +exit: + return ret; +} + +void issue_action_SA_Query(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid) +{ + u8 category = RTW_WLAN_CATEGORY_SA_QUERY; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + __le16 le_tmp; + + DBG_871X("%s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + DBG_871X("%s: alloc_mgtxmitframe fail\n", __func__); + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + if (raddr) + memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + else + memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen); + + switch (action) { + case 0: /* SA Query req */ + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen); + pmlmeext->sa_query_seq++; + /* send sa query request to AP, AP should reply sa query response in 1 second */ + set_sa_query_timer(pmlmeext, 1000); + break; + + case 1: /* SA Query rsp */ + le_tmp = cpu_to_le16(tid); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&le_tmp, &pattrib->pktlen); + break; + default: + break; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +} + +void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status) +{ + u8 category = RTW_WLAN_CATEGORY_BACK; + u16 start_seq; + u16 BA_para_set; + u16 reason_code; + u16 BA_timeout_value; + u16 BA_starting_seqctrl = 0; + enum HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + __le16 le_tmp; + + DBG_871X("%s, category =%d, action =%d, status =%d\n", __func__, category, action, status); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + /* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */ + memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + if (category == 3) { + switch (action) { + case 0: /* ADDBA req */ + do { + pmlmeinfo->dialogToken++; + } while (pmlmeinfo->dialogToken == 0); + pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen)); + + if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter)) { + /* 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) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + } else { + BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */ + } + le_tmp = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + + BA_timeout_value = 5000;/* 5ms */ + le_tmp = cpu_to_le16(BA_timeout_value); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + + /* if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL) */ + psta = rtw_get_stainfo(pstapriv, raddr); + if (psta != NULL) { + start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1; + + DBG_871X("BA_starting_seqctrl = %d for TID =%d\n", start_seq, status & 0x07); + + psta->BA_starting_seqctrl[status & 0x07] = start_seq; + + BA_starting_seqctrl = start_seq << 4; + } + + le_tmp = cpu_to_le16(BA_starting_seqctrl); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + break; + + case 1: /* ADDBA rsp */ + pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen)); + if (padapter->driver_rx_ampdu_factor != 0xFF) + max_rx_ampdu_factor = + (enum HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor; + else + rtw_hal_get_def_var(padapter, + HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); + + if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */ + else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */ + else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor) + BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */ + else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor) + 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 */ + + if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) && + padapter->driver_rx_ampdu_factor == 0xFF) { + /* max buffer size is 8 MSDU */ + BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + } + + if (pregpriv->ampdu_amsdu == 0)/* disabled */ + le_tmp = cpu_to_le16(BA_para_set & ~BIT(0)); + else if (pregpriv->ampdu_amsdu == 1)/* enabled */ + le_tmp = cpu_to_le16(BA_para_set | BIT(0)); + else /* auto */ + le_tmp = cpu_to_le16(BA_para_set); + + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen)); + break; + case 2:/* DELBA */ + BA_para_set = (status & 0x1F) << 3; + le_tmp = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + + reason_code = 37; + le_tmp = cpu_to_le16(reason_code); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen)); + break; + default: + break; + } + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +} + +static void issue_action_BSSCoexistPacket(struct adapter *padapter) +{ + struct list_head *plist, *phead; + unsigned char category, action; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + __le16 *fctrl; + struct wlan_network *pnetwork = NULL; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct __queue *queue = &(pmlmepriv->scanned_queue); + u8 InfoContent[16] = {0}; + u8 ICS[8][15]; + + if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0)) + return; + + if (true == pmlmeinfo->bwmode_updated) + return; + + + DBG_871X("%s\n", __func__); + + + category = RTW_WLAN_CATEGORY_PUBLIC; + action = ACT_PUBLIC_BSSCOEXIST; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_control); + *(fctrl) = 0; + + memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); + memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + + /* */ + if (pmlmepriv->num_FortyMHzIntolerant > 0) { + u8 iedata = 0; + + iedata |= BIT(2);/* 20 MHz BSS Width Request */ + + pframe = rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen)); + + } + + + /* */ + memset(ICS, 0, sizeof(ICS)); + if (pmlmepriv->num_sta_no_ht > 0) { + int i; + + spin_lock_bh(&(pmlmepriv->scanned_queue.lock)); + + phead = get_list_head(queue); + plist = get_next(phead); + + while (1) { + int len; + u8 *p; + struct wlan_bssid_ex *pbss_network; + + if (phead == plist) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + plist = get_next(plist); + + pbss_network = (struct wlan_bssid_ex *)&pnetwork->network; + + p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_); + if ((p == NULL) || (len == 0)) {/* non-HT */ + + if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14)) + continue; + + ICS[0][pbss_network->Configuration.DSConfig] = 1; + + if (ICS[0][0] == 0) + ICS[0][0] = 1; + } + + } + + spin_unlock_bh(&(pmlmepriv->scanned_queue.lock)); + + + for (i = 0; i < 8; i++) { + if (ICS[i][0] == 1) { + int j, k = 0; + + InfoContent[k] = i; + /* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */ + k++; + + for (j = 1; j <= 14; j++) { + if (ICS[i][j] == 1) { + if (k < 16) { + InfoContent[k] = j; /* channel number */ + /* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */ + k++; + } + } + } + + pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen)); + + } + + } + + + } + + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +} + +unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + /* struct recv_reorder_ctrl *preorder_ctrl; */ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u16 tid; + + if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE) + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) + return _SUCCESS; + + psta = rtw_get_stainfo(pstapriv, addr); + if (psta == NULL) + return _SUCCESS; + + /* DBG_871X("%s:%s\n", __func__, (initiator == 0)?"RX_DIR":"TX_DIR"); */ + + if (initiator == 0) {/* recipient */ + for (tid = 0; tid < MAXTID; tid++) { + if (psta->recvreorder_ctrl[tid].enable == true) { + DBG_871X("rx agg disable tid(%d)\n", tid); + issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F)); + psta->recvreorder_ctrl[tid].enable = false; + psta->recvreorder_ctrl[tid].indicate_seq = 0xffff; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__, + psta->recvreorder_ctrl[tid].indicate_seq); + #endif + } + } + } else if (initiator == 1) {/* originator */ + /* DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap); */ + for (tid = 0; tid < MAXTID; tid++) { + if (psta->htpriv.agg_enable_bitmap & BIT(tid)) { + DBG_871X("tx agg disable tid(%d)\n", tid); + issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F)); + psta->htpriv.agg_enable_bitmap &= ~BIT(tid); + psta->htpriv.candidate_tid_bitmap &= ~BIT(tid); + + } + } + } + + return _SUCCESS; + +} + +unsigned int send_beacon(struct adapter *padapter) +{ + u8 bxmitok = false; + int issue = 0; + int poll = 0; + unsigned long start = jiffies; + + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL); + rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL); + do { + issue_beacon(padapter, 100); + issue++; + do { + yield(); + rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok)); + poll++; + } while ((poll%10) != 0 && false == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped); + + } while (false == bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped); + + if (padapter->bSurpriseRemoved || padapter->bDriverStopped) { + return _FAIL; + } + + + if (false == bxmitok) { + DBG_871X("%s fail! %u ms\n", __func__, jiffies_to_msecs(jiffies - start)); + return _FAIL; + } else{ + unsigned long passing_time = jiffies_to_msecs(jiffies - start); + + if (passing_time > 100 || issue > 3) + DBG_871X("%s success, issue:%d, poll:%d, %lu ms\n", __func__, issue, poll, passing_time); + /* else */ + /* DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, passing_time); */ + + return _SUCCESS; + } +} + +/**************************************************************************** + +Following are some utitity fuctions for WiFi MLME + +*****************************************************************************/ + +void site_survey(struct adapter *padapter) +{ + unsigned char survey_channel = 0, val8; + RT_SCAN_TYPE ScanType = SCAN_PASSIVE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u32 initialgain = 0; + u32 channel_scan_time_ms = 0; + + { + struct rtw_ieee80211_channel *ch; + if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) { + ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx]; + survey_channel = ch->hw_value; + ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE; + } + } + + DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n" + , FUNC_ADPT_ARG(padapter) + , survey_channel + , pmlmeext->sitesurvey_res.channel_idx + , jiffies_to_msecs(jiffies - padapter->mlmepriv.scan_start_time) + , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P' + , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' ' + ); +#ifdef DBG_FIXED_CHAN + DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan); +#endif + + if (survey_channel != 0) { + /* PAUSE 4-AC Queue when site_survey */ + /* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + /* val8 |= 0x0f; */ + /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + if (pmlmeext->sitesurvey_res.channel_idx == 0) { +#ifdef DBG_FIXED_CHAN + if (pmlmeext->fixed_chan != 0xff) + set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + else +#endif + set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + } else{ +#ifdef DBG_FIXED_CHAN + if (pmlmeext->fixed_chan != 0xff) + SelectChannel(padapter, pmlmeext->fixed_chan); + else +#endif + SelectChannel(padapter, survey_channel); + } + + if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */ + { + int i; + for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) { + if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) { + /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */ + if (padapter->registrypriv.wifi_spec) + issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL); + else + issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0); + issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL); + } + } + + if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) { + /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */ + if (padapter->registrypriv.wifi_spec) + issue_probereq(padapter, NULL, NULL); + else + issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0); + issue_probereq(padapter, NULL, NULL); + } + } + } + + channel_scan_time_ms = pmlmeext->chan_scan_time; + + set_survey_timer(pmlmeext, channel_scan_time_ms); +#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR) + { + struct noise_info info; + info.bPauseDIG = false; + info.IGIValue = 0; + info.max_time = channel_scan_time_ms/2;/* ms */ + info.chan = survey_channel; + rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &info, false); + } +#endif + + } else{ + + /* channel number is 0 or this channel is not valid. */ + + { + pmlmeext->sitesurvey_res.state = SCAN_COMPLETE; + + /* switch back to the original channel */ + /* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */ + + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + /* flush 4-AC Queue after site_survey */ + /* val8 = 0; */ + /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */ + + /* config MSR */ + Set_MSR(padapter, (pmlmeinfo->state & 0x3)); + + initialgain = 0xff; /* restore RX GAIN */ + rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); + /* turn on dynamic functions */ + Restore_DM_Func_Flag(padapter); + /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */ + + if (is_client_associated_to_ap(padapter) == true) + issue_nulldata(padapter, NULL, 0, 3, 500); + + val8 = 0; /* survey done */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + report_surveydone_event(padapter); + + pmlmeext->chan_scan_time = SURVEY_TO; + pmlmeext->sitesurvey_res.state = SCAN_DISABLE; + + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + } + } + + return; + +} + +/* collect bss info from Beacon and Probe request/response frames. */ +u8 collect_bss_info(struct adapter *padapter, union recv_frame *precv_frame, struct wlan_bssid_ex *bssid) +{ + int i; + u32 len; + u8 *p; + u16 val16, subtype; + u8 *pframe = precv_frame->u.hdr.rx_data; + u32 packet_len = precv_frame->u.hdr.len; + u8 ie_offset; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + __le32 le32_tmp; + + len = packet_len - sizeof(struct ieee80211_hdr_3addr); + + if (len > MAX_IE_SZ) { + /* DBG_871X("IE too long for survey event\n"); */ + return _FAIL; + } + + memset(bssid, 0, sizeof(struct wlan_bssid_ex)); + + subtype = GetFrameSubType(pframe); + + if (subtype == WIFI_BEACON) { + bssid->Reserved[0] = 1; + ie_offset = _BEACON_IE_OFFSET_; + } else { + /* FIXME : more type */ + if (subtype == WIFI_PROBERSP) { + ie_offset = _PROBERSP_IE_OFFSET_; + bssid->Reserved[0] = 3; + } else if (subtype == WIFI_PROBEREQ) { + ie_offset = _PROBEREQ_IE_OFFSET_; + bssid->Reserved[0] = 2; + } else { + bssid->Reserved[0] = 0; + ie_offset = _FIXED_IE_LENGTH_; + } + } + + bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len; + + /* below is to copy the information element */ + bssid->IELength = len; + memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength); + + /* get the signal strength */ + bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; /* in dBM.raw data */ + bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;/* in percentage */ + bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;/* in percentage */ + + /* checking SSID */ + p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset); + if (p == NULL) { + DBG_871X("marc: cannot find SSID for survey event\n"); + return _FAIL; + } + + if (*(p + 1)) { + if (len > NDIS_802_11_LENGTH_SSID) { + DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len); + return _FAIL; + } + memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1)); + bssid->Ssid.SsidLength = *(p + 1); + } else + bssid->Ssid.SsidLength = 0; + + memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX); + + /* checking rate info... */ + i = 0; + p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset); + if (p != NULL) { + if (len > NDIS_802_11_LENGTH_RATES_EX) { + DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len); + return _FAIL; + } + memcpy(bssid->SupportedRates, (p + 2), len); + i = len; + } + + p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset); + if (p != NULL) { + if (len > (NDIS_802_11_LENGTH_RATES_EX-i)) { + DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len); + return _FAIL; + } + memcpy(bssid->SupportedRates + i, (p + 2), len); + } + + bssid->NetworkTypeInUse = Ndis802_11OFDM24; + + if (bssid->IELength < 12) + return _FAIL; + + /* Checking for DSConfig */ + p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset); + + bssid->Configuration.DSConfig = 0; + bssid->Configuration.Length = 0; + + if (p) { + bssid->Configuration.DSConfig = *(p + 2); + } else { + /* In 5G, some ap do not have DSSET IE */ + /* checking HT info for channel */ + p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset); + if (p) { + struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2); + bssid->Configuration.DSConfig = HT_info->primary_channel; + } else { /* use current channel */ + bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter); + } + } + + memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2); + bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp); + + val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid); + + if (val16 & BIT(0)) { + bssid->InfrastructureMode = Ndis802_11Infrastructure; + memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN); + } else { + bssid->InfrastructureMode = Ndis802_11IBSS; + memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN); + } + + if (val16 & BIT(4)) + bssid->Privacy = 1; + else + bssid->Privacy = 0; + + bssid->Configuration.ATIMWindow = 0; + + /* 20/40 BSS Coexistence check */ + if ((pregistrypriv->wifi_spec == 1) && (false == pmlmeinfo->bwmode_updated)) { + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset); + if (p && len > 0) { + struct HT_caps_element *pHT_caps; + pHT_caps = (struct HT_caps_element *)(p + 2); + + if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & BIT(14)) + pmlmepriv->num_FortyMHzIntolerant++; + } else + pmlmepriv->num_sta_no_ht++; + } + +#ifdef CONFIG_INTEL_WIDI + /* process_intel_widi_query_or_tigger(padapter, bssid); */ + if (process_intel_widi_query_or_tigger(padapter, bssid)) + return _FAIL; +#endif /* CONFIG_INTEL_WIDI */ + + #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1 + if (strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n" + , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig + , rtw_get_oper_ch(padapter) + , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi + ); + } + #endif + + /* mark bss info receving from nearby channel as SignalQuality 101 */ + if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter)) + bssid->PhyInfo.SignalQuality = 101; + + return _SUCCESS; +} + +void start_create_ibss(struct adapter *padapter) +{ + unsigned short caps; + u8 val8; + u8 join_type; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig; + pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork); + + /* update wireless mode */ + update_wireless_mode(padapter); + + /* udpate capability */ + caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork); + update_capinfo(padapter, caps); + if (caps&cap_IBSS) {/* adhoc master */ + val8 = 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL); + + /* switch channel */ + /* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */ + set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + + beacon_timing_control(padapter); + + /* set msr to WIFI_FW_ADHOC_STATE */ + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + Set_MSR(padapter, (pmlmeinfo->state & 0x3)); + + /* issue beacon */ + if (send_beacon(padapter) == _FAIL) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n")); + + report_join_res(padapter, -1); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + } else{ + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress); + join_type = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + report_join_res(padapter, 1); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + rtw_indicate_connect(padapter); + } + } else{ + DBG_871X("start_create_ibss, invalid cap:%x\n", caps); + return; + } + /* update bc/mc sta_info */ + update_bmc_sta(padapter); + +} + +void start_clnt_join(struct adapter *padapter) +{ + unsigned short caps; + u8 val8; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + int beacon_timeout; + + /* update wireless mode */ + update_wireless_mode(padapter); + + /* udpate capability */ + caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork); + update_capinfo(padapter, caps); + if (caps&cap_ESS) { + Set_MSR(padapter, WIFI_FW_STATION_STATE); + + val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf; + + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* 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. */ + { + /* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */ + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100); + } + + /* here wait for receiving the beacon to start auth */ + /* and enable a timer */ + beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval); + set_link_timer(pmlmeext, beacon_timeout); + _set_timer(&padapter->mlmepriv.assoc_timer, + (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout); + + pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE; + } else if (caps&cap_IBSS) { /* adhoc client */ + Set_MSR(padapter, WIFI_FW_ADHOC_STATE); + + val8 = 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + beacon_timing_control(padapter); + + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + + report_join_res(padapter, 1); + } else{ + /* DBG_871X("marc: invalid cap:%x\n", caps); */ + return; + } + +} + +void start_clnt_auth(struct 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; + + + DBG_871X_LEVEL(_drv_always_, "start auth\n"); + issue_auth(padapter, NULL, 0); + + set_link_timer(pmlmeext, REAUTH_TO); + +} + + +void start_clnt_assoc(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + 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_assocreq(padapter); + + set_link_timer(pmlmeext, REASSOC_TO); +} + +unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* check A3 */ + if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN))) + return _SUCCESS; + + DBG_871X("%s\n", __func__); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) { + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_del_sta_event(padapter, MacAddr, reason); + + } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) { + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res(padapter, -2); + } + } + + return _SUCCESS; +} + +static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid) +{ + struct registry_priv *pregistrypriv; + struct mlme_ext_priv *pmlmeext; + 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; + RT_CHANNEL_PLAN chplan_ap; + RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM]; + u8 country[4]; + u8 fcn; /* first channel number */ + u8 noc; /* number of channel */ + u8 j, k; + + ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (!ie) + return; + if (len < 6) + return; + + ie += 2; + p = ie; + ie += len; + + memset(country, 0, 4); + memcpy(country, p, 3); + p += 3; + RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, + ("%s: 802.11d country =%s\n", __func__, country)); + + i = 0; + while ((ie - p) >= 3) { + fcn = *(p++); + noc = *(p++); + p++; + + for (j = 0; j < noc; j++) { + if (fcn <= 14) + channel = fcn + j; /* 2.4 GHz */ + else + channel = fcn + j*4; /* 5 GHz */ + + chplan_ap.Channel[i++] = channel; + } + } + chplan_ap.Len = i; + +#ifdef DEBUG_RTL871X + i = 0; + DBG_871X("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid); + while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0)) { + DBG_8192C("%02d,", chplan_ap.Channel[i]); + i++; + } + DBG_871X("}\n"); +#endif + + memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta)); +#ifdef DEBUG_RTL871X + i = 0; + DBG_871X("%s: STA channel plan {", __func__); + while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) { + DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType == SCAN_PASSIVE?'p':'a'); + i++; + } + DBG_871X("}\n"); +#endif + + 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 = 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_sta[i].ChannelNum <= 14)) { + + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; +/* chplan_new[k].ScanType = chplan_sta[i].ScanType; */ + 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 = chplan_sta[i].ScanType; */ + 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; + +#ifdef DEBUG_RTL871X + k = 0; + DBG_871X("%s: new STA channel plan {", __func__); + while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) { + DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType == SCAN_PASSIVE?'p':'c'); + k++; + } + DBG_871X("}\n"); +#endif + } + + /* 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_event(struct adapter *padapter, union recv_frame *precv_frame) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct survey_event *psurvey_evt; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext; + struct cmd_priv *pcmdpriv; + /* u8 *pframe = precv_frame->u.hdr.rx_data; */ + /* uint len = precv_frame->u.hdr.len; */ + + if (!padapter) + return; + + pmlmeext = &padapter->mlmeextpriv; + pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)pcmd_obj); + return; + } + + INIT_LIST_HEAD(&pcmd_obj->list); + + pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT); + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd); + pc2h_evt_hdr->len = sizeof(struct survey_event); + pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey); + pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq); + + psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header)); + + if (collect_bss_info(padapter, precv_frame, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) { + kfree((u8 *)pcmd_obj); + kfree((u8 *)pevtcmd); + return; + } + + process_80211d(padapter, &psurvey_evt->bss); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + pmlmeext->sitesurvey_res.bss_cnt++; + + return; + +} + +void report_surveydone_event(struct adapter *padapter) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct surveydone_event *psurveydone_evt; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)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_871X("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter)); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; + +} + +void report_join_res(struct adapter *padapter, int res) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct joinbss_event *pjoinbss_evt; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)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_871X("report_join_res(%d)\n", res); + + + rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network); + + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; + +} + +void report_wmm_edca_update(struct adapter *padapter) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct wmm_event *pwmm_event; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)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 wmm_event); + pc2h_evt_hdr->ID = GEN_EVT_CODE(_WMM); + pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq); + + pwmm_event = (struct wmm_event *)(pevtcmd + sizeof(struct C2HEvent_Header)); + pwmm_event->wmm = 0; + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; + +} + +void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct sta_info *psta; + int mac_id; + struct stadel_event *pdel_sta_evt; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) { + return; + } + + cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)pcmd_obj); + return; + } + + INIT_LIST_HEAD(&pcmd_obj->list); + + pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT); + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd); + pc2h_evt_hdr->len = sizeof(struct stadel_event); + pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA); + pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq); + + pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header)); + memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN); + memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2); + + + psta = rtw_get_stainfo(&padapter->stapriv, MacAddr); + if (psta) + mac_id = (int)psta->mac_id; + else + mac_id = (-1); + + pdel_sta_evt->mac_id = mac_id; + + DBG_871X("report_del_sta_event: delete STA, mac_id =%d\n", mac_id); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; +} + +void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct stassoc_event *padd_sta_evt; + struct C2HEvent_Header *pc2h_evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + kfree((u8 *)pcmd_obj); + return; + } + + INIT_LIST_HEAD(&pcmd_obj->list); + + pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT); + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd); + pc2h_evt_hdr->len = sizeof(struct stassoc_event); + pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA); + pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq); + + padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header)); + memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN); + padd_sta_evt->cam_id = cam_idx; + + DBG_871X("report_add_sta_event: add STA\n"); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; +} + + +bool rtw_port_switch_chk(struct adapter *adapter) +{ + bool switch_needed = false; + return switch_needed; +} + +/**************************************************************************** + +Following are the event callback functions + +*****************************************************************************/ + +/* for sta/adhoc mode */ +void update_sta_info(struct adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* ERP */ + VCS_update(padapter, psta); + + /* HT */ + if (pmlmepriv->htpriv.ht_option) { + psta->htpriv.ht_option = true; + + psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable; + + psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para&IEEE80211_HT_CAP_AMPDU_DENSITY)>>2; + + if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20)) + psta->htpriv.sgi_20m = true; + + if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40)) + psta->htpriv.sgi_40m = true; + + psta->qos_option = true; + + psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap; + psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap; + psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap; + + memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap)); + } else{ + psta->htpriv.ht_option = false; + + psta->htpriv.ampdu_enable = false; + + psta->htpriv.sgi_20m = false; + psta->htpriv.sgi_40m = false; + psta->qos_option = false; + + } + + psta->htpriv.ch_offset = pmlmeext->cur_ch_offset; + + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + + psta->bw_mode = pmlmeext->cur_bwmode; + + /* QoS */ + if (pmlmepriv->qospriv.qos_option) + psta->qos_option = true; + + update_ldpc_stbc_cap(psta); + + spin_lock_bh(&psta->lock); + psta->state = _FW_LINKED; + spin_unlock_bh(&psta->lock); + +} + +static void rtw_mlmeext_disconnect(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + u8 state_backup = (pmlmeinfo->state&0x03); + + /* set_opmode_cmd(padapter, infra_client_with_mlme); */ + + /* + * For safety, prevent from keeping macid sleep. + * If we can sure all power mode enter/leave are paired, + * this check can be removed. + * Lucas@20131113 + */ + /* wakeup macid after disconnect. */ + { + struct sta_info *psta; + psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork)); + if (psta) + rtw_hal_macid_wakeup(padapter, psta->mac_id); + } + + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL); + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr); + + /* set MSR to no link state -> infra. mode */ + Set_MSR(padapter, _HW_STATE_STATION_); + + pmlmeinfo->state = WIFI_FW_NULL_STATE; + + if (state_backup == WIFI_FW_STATION_STATE) { + if (rtw_port_switch_chk(padapter) == true) { + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); + { + struct adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter)); + if (port0_iface) + rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0); + } + } + } + + /* switch to the 20M Hz mode after disconnect */ + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + flush_all_cam_entry(padapter); + + del_timer_sync(&pmlmeext->link_timer); + + /* pmlmepriv->LinkDetectInfo.TrafficBusyState = false; */ + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0; + +} + +void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res) +{ + 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; + struct sta_info *psta; + if (join_res < 0) { + join_type = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr); + + goto exit_mlmeext_joinbss_event_callback; + } + + if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) + /* update bc/mc sta_info */ + update_bmc_sta(padapter); + + + /* turn on dynamic functions */ + Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); + + /* update IOT-releated issue */ + update_IOT_info(padapter); + + rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates); + + /* BCN interval */ + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval)); + + /* udpate capability */ + update_capinfo(padapter, pmlmeinfo->capability); + + /* WMM, Update EDCA param */ + WMMOnAssocRsp(padapter); + + /* HT */ + HTOnAssocRsp(padapter); + + /* Set cur_channel&cur_bwmode&cur_ch_offset */ + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (psta) { /* only for infra. mode */ + + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + /* DBG_871X("set_sta_rate\n"); */ + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + + /* set per sta rate after updating HT cap. */ + set_sta_rate(padapter, psta); + + rtw_sta_media_status_rpt(padapter, psta, 1); + + /* wakeup macid after join bss successfully to ensure + the subsequent data frames can be sent out normally */ + rtw_hal_macid_wakeup(padapter, psta->mac_id); + } + + if (rtw_port_switch_chk(padapter) == true) + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); + + join_type = 2; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) { + /* correcting TSF */ + correct_TSF(padapter, pmlmeext); + + /* set_link_timer(pmlmeext, DISCONNECT_TO); */ + } + + if (get_iface_type(padapter) == IFACE_PORT0) + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0); + +exit_mlmeext_joinbss_event_callback: + + DBG_871X("=>%s\n", __func__); + +} + +/* currently only adhoc mode will go here */ +void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 join_type; + + DBG_871X("%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_TSF(pmlmeext, pframe, len); */ + + /* correcting TSF */ + correct_TSF(padapter, pmlmeext); + + /* start beacon */ + if (send_beacon(padapter) == _FAIL) { + pmlmeinfo->FW_sta_info[psta->mac_id].status = 0; + + pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE; + + return; + } + + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + + } + + join_type = 2; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + } + + pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta; + + psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates); + memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen); + + /* update adhoc sta_info */ + update_sta_info(padapter, psta); + + rtw_hal_update_sta_rate_mask(padapter, psta); + + /* ToDo: HT for Ad-hoc */ + psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel); + psta->raid = rtw_hal_networktype_to_raid(padapter, psta); + + /* rate radaptive */ + Update_RA_Entry(padapter, psta); +} + +void mlmeext_sta_del_event_callback(struct adapter *padapter) +{ + if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter)) + rtw_mlmeext_disconnect(padapter); +} + +/**************************************************************************** + +Following are the functions for the timer handlers + +*****************************************************************************/ +void _linked_info_dump(struct adapter *padapter) +{ + int i; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + int UndecoratedSmoothedPWDB; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + + if (padapter->bLinkInfoDump) { + + DBG_871X("\n ============["ADPT_FMT"] linked status check ===================\n", ADPT_ARG(padapter)); + + if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) { + rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB); + + DBG_871X("AP[" MAC_FMT "] - UndecoratedSmoothedPWDB:%d\n", + MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress), UndecoratedSmoothedPWDB); + } else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_) { + struct list_head *phead, *plist; + + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + while (phead != plist) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + DBG_871X("STA[" MAC_FMT "]:UndecoratedSmoothedPWDB:%d\n", + MAC_ARG(psta->hwaddr), psta->rssi_stat.UndecoratedSmoothedPWDB); + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + } + for (i = 0; i < NUM_STA; i++) { + if (pdvobj->macid[i] == true) { + if (i != 1) /* skip bc/mc sta */ + /* tx info ============ */ + rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i); + } + } + rtw_hal_set_def_var(padapter, HAL_DEF_DBG_RX_INFO_DUMP, NULL); + + + } + + +} + +static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta) +{ + u8 ret = false; + + #ifdef DBG_EXPIRATION_CHK + DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu" + /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/ + ", retry:%u\n" + , FUNC_ADPT_ARG(padapter) + , STA_RX_PKTS_DIFF_ARG(psta) + , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts + , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts + /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts + , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts + , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts + , pmlmeinfo->bcn_interval*/ + , pmlmeext->retry + ); + + DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter) + , padapter->xmitpriv.tx_pkts + , pmlmeinfo->link_count + ); + #endif + + if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) + && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) + && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta) + ) { + ret = false; + } else{ + ret = true; + } + + sta_update_last_rx_pkts(psta); + + return ret; +} + +void linked_status_chk(struct adapter *padapter) +{ + u32 i; + struct sta_info *psta; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_priv *pstapriv = &padapter->stapriv; + + + if (is_client_associated_to_ap(padapter)) { + /* linked infrastructure client mode */ + + int tx_chk = _SUCCESS, rx_chk = _SUCCESS; + int rx_chk_limit; + int link_count_limit; + + #if defined(DBG_ROAMING_TEST) + rx_chk_limit = 1; + #else + rx_chk_limit = 8; + #endif + link_count_limit = 7; /* 16 sec */ + + /* Marked by Kurt 20130715 */ + /* For WiDi 3.5 and latered on, they don't ask WiDi sink to do roaming, so we could not check rx limit that strictly. */ + /* todo: To check why we under miracast session, rx_chk would be false */ + /* ifdef CONFIG_INTEL_WIDI */ + /* if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE) */ + /* rx_chk_limit = 1; */ + /* endif */ + + psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress); + if (psta != NULL) { + if (chk_ap_is_alive(padapter, psta) == false) + rx_chk = _FAIL; + + if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts) + tx_chk = _FAIL; + + { + if (rx_chk != _SUCCESS) { + if (pmlmeext->retry == 0) { + #ifdef DBG_EXPIRATION_CHK + DBG_871X("issue_probereq to trigger probersp, retry =%d\n", pmlmeext->retry); + #endif + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0); + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0); + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0); + } + } + + if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) { + #ifdef DBG_EXPIRATION_CHK + DBG_871X("%s issue_nulldata 0\n", __func__); + #endif + tx_chk = issue_nulldata_in_interrupt(padapter, NULL); + } + } + + if (rx_chk == _FAIL) { + pmlmeext->retry++; + if (pmlmeext->retry > rx_chk_limit) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n", + FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress + , WLAN_REASON_EXPIRATION_CHK); + return; + } + } else { + pmlmeext->retry = 0; + } + + if (tx_chk == _FAIL) { + pmlmeinfo->link_count %= (link_count_limit+1); + } else { + pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts; + pmlmeinfo->link_count = 0; + } + + } /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */ + } else if (is_client_associated_to_ibss(padapter)) { + /* linked IBSS mode */ + /* for each assoc list entry to check the rx pkt counter */ + for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) { + if (pmlmeinfo->FW_sta_info[i].status == 1) { + psta = pmlmeinfo->FW_sta_info[i].psta; + + if (NULL == psta) + continue; + + if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) { + + if (pmlmeinfo->FW_sta_info[i].retry < 3) { + pmlmeinfo->FW_sta_info[i].retry++; + } else{ + pmlmeinfo->FW_sta_info[i].retry = 0; + pmlmeinfo->FW_sta_info[i].status = 0; + report_del_sta_event(padapter, psta->hwaddr + , 65535/* indicate disconnect caused by no rx */ + ); + } + } else{ + pmlmeinfo->FW_sta_info[i].retry = 0; + pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta); + } + } + } + + /* set_link_timer(pmlmeext, DISCONNECT_TO); */ + + } + +} + +void survey_timer_hdl(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct sitesurvey_parm *psurveyPara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + /* DBG_871X("marc: survey timer\n"); */ + + /* issue rtw_sitesurvey_cmd */ + if (pmlmeext->sitesurvey_res.state > SCAN_START) { + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + pmlmeext->sitesurvey_res.channel_idx++; + } + + if (pmlmeext->scan_abort == true) { + { + pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num; + DBG_871X("%s idx:%d\n", __func__ + , pmlmeext->sitesurvey_res.channel_idx + ); + } + + pmlmeext->scan_abort = false;/* reset */ + } + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + goto exit_survey_timer_hdl; + } + + psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm)); + if (psurveyPara == NULL) { + kfree((unsigned char *)ph2c); + goto exit_survey_timer_hdl; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey)); + rtw_enqueue_cmd(pcmdpriv, ph2c); + } + + +exit_survey_timer_hdl: + + return; +} + +void link_timer_hdl(struct adapter *padapter) +{ + /* 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_871X("link_timer_hdl:no beacon while connecting\n"); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res(padapter, -3); + } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) { + /* re-auth timer */ + if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) { + /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */ + /* */ + pmlmeinfo->state = 0; + report_join_res(padapter, -1); + return; + /* */ + /* else */ + /* */ + /* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */ + /* pmlmeinfo->reauth_count = 0; */ + /* */ + } + + DBG_871X("link_timer_hdl: auth timeout and try again\n"); + pmlmeinfo->auth_seq = 1; + issue_auth(padapter, NULL, 0); + set_link_timer(pmlmeext, REAUTH_TO); + } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) { + /* re-assoc timer */ + if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) { + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res(padapter, -2); + return; + } + + DBG_871X("link_timer_hdl: assoc timeout and try again\n"); + issue_assocreq(padapter); + set_link_timer(pmlmeext, REASSOC_TO); + } + + return; +} + +void addba_timer_hdl(struct sta_info *psta) +{ + struct ht_priv *phtpriv; + + if (!psta) + return; + + phtpriv = &psta->htpriv; + + if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) { + if (phtpriv->candidate_tid_bitmap) + phtpriv->candidate_tid_bitmap = 0x0; + + } +} + +void sa_query_timer_hdl(struct adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + /* disconnect */ + spin_lock_bh(&pmlmepriv->lock); + + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) { + rtw_disassoc_cmd(padapter, 0, true); + rtw_indicate_disconnect(padapter); + rtw_free_assoc_resources(padapter, 1); + } + + spin_unlock_bh(&pmlmepriv->lock); + DBG_871X("SA query timeout disconnect\n"); +} + +u8 NULL_hdl(struct adapter *padapter, u8 *pbuf) +{ + return H2C_SUCCESS; +} + +#ifdef CONFIG_AUTO_AP_MODE +static int rtw_auto_ap_start_beacon(struct adapter *adapter) +{ + int ret = 0; + u8 *pbuf = NULL; + uint len; + u8 supportRate[16]; + int sz = 0, rateLen; + u8 *ie; + u8 wireless_mode, oper_channel; + u8 ssid[3] = {0}; /* hidden ssid */ + u32 ssid_len = sizeof(ssid); + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true) + return -EINVAL; + + + len = 128; + pbuf = rtw_zmalloc(len); + if (!pbuf) + return -ENOMEM; + + + /* generate beacon */ + ie = pbuf; + + /* timestamp will be inserted by hardware */ + sz += 8; + ie += sz; + + /* beacon interval : 2bytes */ + *(u16 *)ie = cpu_to_le16((u16)100);/* BCN_INTERVAL = 100; */ + sz += 2; + ie += 2; + + /* capability info */ + *(u16 *)ie = 0; + *(u16 *)ie |= cpu_to_le16(cap_ESS); + *(u16 *)ie |= cpu_to_le16(cap_ShortPremble); + /* u16*)ie |= cpu_to_le16(cap_Privacy); */ + sz += 2; + ie += 2; + + /* SSID */ + ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz); + + /* supported rates */ + wireless_mode = WIRELESS_11BG_24N; + rtw_set_supported_rate(supportRate, wireless_mode); + rateLen = rtw_get_rateset_len(supportRate); + if (rateLen > 8) { + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz); + } else{ + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz); + } + + + /* DS parameter set */ + if (check_buddy_fwstate(adapter, _FW_LINKED) && + check_buddy_fwstate(adapter, WIFI_STATION_STATE)) { + struct adapter *pbuddystruct adapter = adapter->pbuddystruct adapter; + struct mlme_ext_priv *pbuddy_mlmeext = &pbuddystruct adapter->mlmeextpriv; + + oper_channel = pbuddy_mlmeext->cur_channel; + } else{ + oper_channel = adapter_to_dvobj(adapter)->oper_channel; + } + ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz); + + /* ext supported rates */ + if (rateLen > 8) { + ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz); + } + + DBG_871X("%s, start auto ap beacon sz =%d\n", __func__, sz); + + /* lunch ap mode & start to issue beacon */ + if (rtw_check_beacon_data(adapter, pbuf, sz) == _SUCCESS) { + + } else{ + ret = -EINVAL; + } + + + kfree(pbuf); + + return ret; + +} +#endif/* CONFIG_AUTO_AP_MODE */ + +u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf) +{ + u8 type; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf; + + if (psetop->mode == Ndis802_11APMode) { + pmlmeinfo->state = WIFI_FW_AP_STATE; + type = _HW_STATE_AP_; + /* start_ap_mode(padapter); */ + } else if (psetop->mode == Ndis802_11Infrastructure) { + pmlmeinfo->state &= ~(BIT(0)|BIT(1));/* clear state */ + pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to STATION_STATE */ + type = _HW_STATE_STATION_; + } else if (psetop->mode == Ndis802_11IBSS) { + type = _HW_STATE_ADHOC_; + } else{ + type = _HW_STATE_NOLINK_; + } + + rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type)); + /* Set_NETYPE0_MSR(padapter, type); */ + + +#ifdef CONFIG_AUTO_AP_MODE + if (psetop->mode == Ndis802_11APMode) + rtw_auto_ap_start_beacon(padapter); +#endif + + if (rtw_port_switch_chk(padapter) == true) { + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); + + if (psetop->mode == Ndis802_11APMode) + adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; /* ap mode won't dowload rsvd pages */ + else if (psetop->mode == Ndis802_11Infrastructure) { + struct adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter)); + if (port0_iface) + rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0); + } + } + + if (psetop->mode == Ndis802_11APMode) { + /* Do this after port switch to */ + /* prevent from downloading rsvd page to wrong port */ + rtw_btcoex_MediaStatusNotify(padapter, 1); /* connect */ + } + + return H2C_SUCCESS; + +} + +u8 createbss_hdl(struct adapter *padapter, u8 *pbuf) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf; + /* u32 initialgain; */ + + if (pmlmeinfo->state == WIFI_FW_AP_STATE) { + struct wlan_bssid_ex *network = &padapter->mlmepriv.cur_network.network; + start_bss_network(padapter, (u8 *)network); + return H2C_SUCCESS; + } + + /* below is for ad-hoc master */ + if (pparm->network.InfrastructureMode == Ndis802_11IBSS) { + rtw_joinbss_reset(padapter); + + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pmlmeinfo->ERP_enable = 0; + pmlmeinfo->WMM_enable = 0; + pmlmeinfo->HT_enable = 0; + pmlmeinfo->HT_caps_enable = 0; + pmlmeinfo->HT_info_enable = 0; + pmlmeinfo->agg_enable_bitmap = 0; + pmlmeinfo->candidate_tid_bitmap = 0; + + /* disable dynamic functions, such as high power, DIG */ + Save_DM_Func_Flag(padapter); + Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); + + /* config the initial gain under linking, need to write the BB registers */ + /* initialgain = 0x1E; */ + /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */ + + /* cancel link timer */ + del_timer_sync(&pmlmeext->link_timer); + + /* clear CAM */ + flush_all_cam_entry(padapter); + + memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength)); + pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength; + + if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */ + return H2C_PARAMETERS_ERROR; + + memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength); + + start_create_ibss(padapter); + + } + + return H2C_SUCCESS; + +} + +u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf) +{ + u8 join_type; + struct ndis_80211_var_ie *pIE; + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + u32 i; + u8 cbw40_enable = 0; + /* u32 initialgain; */ + /* u32 acparm; */ + u8 ch, bw, offset; + + /* check already connecting to AP or not */ + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { + if (pmlmeinfo->state & WIFI_FW_STATION_STATE) { + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100); + } + pmlmeinfo->state = WIFI_FW_NULL_STATE; + + /* clear CAM */ + flush_all_cam_entry(padapter); + + del_timer_sync(&pmlmeext->link_timer); + + /* set MSR to nolink -> infra. mode */ + /* Set_MSR(padapter, _HW_STATE_NOLINK_); */ + Set_MSR(padapter, _HW_STATE_STATION_); + + + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL); + } + + rtw_joinbss_reset(padapter); + + pmlmeext->cur_bwmode = 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; */ + pmlmeinfo->VHT_enable = 0; + + memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength)); + pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength; + + if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */ + return H2C_PARAMETERS_ERROR; + + memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength); + + pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig; + pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork); + + /* Check AP vendor to move rtw_joinbss_cmd() */ + /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength); */ + + /* sizeof(struct ndis_802_11_fix_ie) */ + for (i = _FIXED_IE_LENGTH_; i < pnetwork->IELength;) { + pIE = (struct ndis_80211_var_ie *)(pnetwork->IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */ + if (!memcmp(pIE->data, WMM_OUI, 4)) + WMM_param_handler(padapter, pIE); + break; + + case _HT_CAPABILITY_IE_: /* Get HT Cap IE. */ + pmlmeinfo->HT_caps_enable = 1; + break; + + case _HT_EXTRA_INFO_IE_: /* Get HT Info IE. */ + pmlmeinfo->HT_info_enable = 1; + + /* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */ + { + struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data); + + if (pnetwork->Configuration.DSConfig > 14) { + if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20) + cbw40_enable = 1; + } else { + if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20) + cbw40_enable = 1; + } + + if ((cbw40_enable) && (pht_info->infos[0] & BIT(2))) { + /* switch to the 40M Hz mode according to the AP */ + pmlmeext->cur_bwmode = 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; + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + break; + } + + DBG_871X("set HT ch/bw before connected\n"); + } + } + break; + default: + break; + } + + i += (pIE->Length + 2); + } + + /* check channel, bandwidth, offset and switch */ + if (rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) { + report_join_res(padapter, (-4)); + return H2C_SUCCESS; + } + + /* disable dynamic functions, such as high power, DIG */ + /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */ + + /* config the initial gain under linking, need to write the BB registers */ + /* initialgain = 0x1E; */ + /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */ + + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress); + join_type = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL); + + set_channel_bwmode(padapter, ch, offset, bw); + + /* cancel link timer */ + del_timer_sync(&pmlmeext->link_timer); + + start_clnt_join(padapter); + + return H2C_SUCCESS; + +} + +u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct disconnect_parm *param = (struct disconnect_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network)); + u8 val8; + + if (is_client_associated_to_ap(padapter)) { + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100); + } + + if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) { + /* Stop BCN */ + val8 = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8)); + } + + rtw_mlmeext_disconnect(padapter); + + rtw_free_uc_swdec_pending_queue(padapter); + + return H2C_SUCCESS; +} + +static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out, + u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num) +{ + int i, j; + int set_idx; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + /* clear first */ + memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num); + + /* acquire channels from in */ + j = 0; + for (i = 0; i < in_num; i++) { + + DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i])); + + set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value); + if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED) + && set_idx >= 0 + && rtw_mlme_band_check(padapter, in[i].hw_value) == true + ) { + if (j >= out_num) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n", + FUNC_ADPT_ARG(padapter), out_num); + break; + } + + memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel)); + + if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE) + out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN; + + j++; + } + if (j >= out_num) + break; + } + + /* if out is empty, use channel_set as default */ + if (j == 0) { + for (i = 0; i < pmlmeext->max_chan_nums; i++) { + + DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum); + + if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == true) { + + if (j >= out_num) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n", + FUNC_ADPT_ARG(padapter), out_num); + break; + } + + out[j].hw_value = pmlmeext->channel_set[i].ChannelNum; + + if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE) + out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN; + + j++; + } + } + } + + return j; +} + +u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf; + u8 bdelayscan = false; + u8 val8; + u32 initialgain; + u32 i; + + if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) { + pmlmeext->sitesurvey_res.state = SCAN_START; + pmlmeext->sitesurvey_res.bss_cnt = 0; + pmlmeext->sitesurvey_res.channel_idx = 0; + + for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) { + if (pparm->ssid[i].SsidLength) { + memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE); + pmlmeext->sitesurvey_res.ssid[i].SsidLength = pparm->ssid[i].SsidLength; + } else { + pmlmeext->sitesurvey_res.ssid[i].SsidLength = 0; + } + } + + pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter + , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT + , pparm->ch, pparm->ch_num + ); + + pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode; + + /* issue null data if associating to the AP */ + if (is_client_associated_to_ap(padapter) == true) { + pmlmeext->sitesurvey_res.state = SCAN_TXNULL; + + issue_nulldata(padapter, NULL, 1, 3, 500); + + bdelayscan = true; + } + if (bdelayscan) { + /* delay 50ms to protect nulldata(1). */ + set_survey_timer(pmlmeext, 50); + return H2C_SUCCESS; + } + } + + if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) { + /* disable dynamic functions, such as high power, DIG */ + Save_DM_Func_Flag(padapter); + Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); + + /* config the initial gain under scaning, need to write the BB registers */ + initialgain = 0x1e; + + rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); + + /* set MSR to no link state */ + Set_MSR(padapter, _HW_STATE_NOLINK_); + + val8 = 1; /* under site survey */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + pmlmeext->sitesurvey_res.state = SCAN_PROCESS; + } + + site_survey(padapter); + + return H2C_SUCCESS; + +} + +u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct setauth_parm *pparm = (struct setauth_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pparm->mode < 4) { + pmlmeinfo->auth_algo = pparm->mode; + } + + return H2C_SUCCESS; +} + +u8 setkey_hdl(struct adapter *padapter, u8 *pbuf) +{ + u16 ctrl = 0; + s16 cam_id = 0; + 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_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + u8 *addr; + + /* main tx key for wep. */ + if (pparm->set_tx) + pmlmeinfo->key_index = pparm->keyid; + + cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid); + + if (cam_id < 0) { + } else { + if (cam_id > 3) /* not default key, searched by A2 */ + addr = get_bssid(&padapter->mlmepriv); + else + addr = null_addr; + + ctrl = BIT(15) | BIT6 | ((pparm->algorithm) << 2) | pparm->keyid; + write_cam(padapter, cam_id, ctrl, addr, pparm->key); + DBG_871X_LEVEL(_drv_always_, "set group key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n" + , cam_id, MAC_ARG(addr), pparm->keyid, security_type_str(pparm->algorithm)); + } + + if (cam_id >= 0 && cam_id <= 3) + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)true); + + /* allow multicast packets to driver */ + padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr); + + return H2C_SUCCESS; +} + +u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf) +{ + u16 ctrl = 0; + s16 cam_id = 0; + u8 ret = H2C_SUCCESS; + 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; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + + if (pparm->algorithm == _NO_PRIVACY_) + goto write_to_cam; + + psta = rtw_get_stainfo(pstapriv, pparm->addr); + if (!psta) { + DBG_871X_LEVEL(_drv_always_, "%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr)); + ret = H2C_REJECTED; + goto exit; + } + + pmlmeinfo->enc_algo = pparm->algorithm; + cam_id = rtw_camid_alloc(padapter, psta, 0); + if (cam_id < 0) + goto exit; + +write_to_cam: + if (pparm->algorithm == _NO_PRIVACY_) { + while ((cam_id = rtw_camid_search(padapter, pparm->addr, -1)) >= 0) { + DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id); + clear_cam_entry(padapter, cam_id); + rtw_camid_free(padapter, cam_id); + } + } else { + DBG_871X_LEVEL(_drv_always_, "set pairwise key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n", + cam_id, MAC_ARG(pparm->addr), pparm->keyid, security_type_str(pparm->algorithm)); + ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid; + write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key); + } + ret = H2C_SUCCESS_RSP; + +exit: + return ret; +} + +u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr); + + if (!psta) + return H2C_SUCCESS; + + if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) || + ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) { + /* pmlmeinfo->ADDBA_retry_count = 0; */ + /* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid); */ + /* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */ + issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid); + /* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */ + _set_timer(&psta->addba_retry_timer, ADDBA_TO); + } else{ + psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid); + } + return H2C_SUCCESS; +} + + +u8 chk_bmc_sleepq_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_parm_rsp(ph2c, GEN_CMD_CODE(_ChkBMCSleepq)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 set_tx_beacon_cmd(struct adapter *padapter) +{ + struct cmd_obj *ph2c; + struct Tx_Beacon_param *ptxBeacon_parm; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 res = _SUCCESS; + int len_diff = 0; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param)); + if (ptxBeacon_parm == NULL) { + kfree((unsigned char *)ph2c); + res = _FAIL; + goto exit; + } + + memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex)); + + len_diff = update_hidden_ssid( + ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_ + , ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_ + , pmlmeinfo->hidden_ssid_mode + ); + ptxBeacon_parm->network.IELength += len_diff; + + init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon)); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + + +u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + u8 evt_code, evt_seq; + u16 evt_sz; + uint *peventbuf; + void (*event_callback)(struct adapter *dev, u8 *pbuf); + struct evt_priv *pevt_priv = &(padapter->evtpriv); + + if (pbuf == NULL) + goto _abort_event_; + + peventbuf = (uint *)pbuf; + evt_sz = (u16)(*peventbuf&0xffff); + evt_seq = (u8)((*peventbuf>>24)&0x7f); + evt_code = (u8)((*peventbuf>>16)&0xff); + + + #ifdef CHECK_EVENT_SEQ + /* checking event sequence... */ + if (evt_seq != (atomic_read(&pevt_priv->event_seq) & 0x7f)) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, + ("Event Seq Error! %d vs %d\n", (evt_seq & 0x7f), + (atomic_read(&pevt_priv->event_seq) & 0x7f))); + + pevt_priv->event_seq = (evt_seq+1)&0x7f; + + goto _abort_event_; + } + #endif + + /* checking if event code is valid */ + if (evt_code >= MAX_C2HEVT) { + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code)); + goto _abort_event_; + } + + /* checking if event size match the event parm size */ + if ((wlanevents[evt_code].parmsize != 0) && + (wlanevents[evt_code].parmsize != evt_sz)) { + + RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n", + evt_code, wlanevents[evt_code].parmsize, evt_sz)); + goto _abort_event_; + + } + + atomic_inc(&pevt_priv->event_seq); + + peventbuf += 2; + + if (peventbuf) { + event_callback = wlanevents[evt_code].event_callback; + event_callback(padapter, (u8 *)peventbuf); + + pevt_priv->evt_done_cnt++; + } + + +_abort_event_: + + + return H2C_SUCCESS; + +} + +u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + return H2C_SUCCESS; +} + +u8 chk_bmc_sleepq_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct sta_info *psta_bmc; + struct list_head *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + return H2C_SUCCESS; + + if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len > 0)) { + msleep(10);/* 10ms, ATIM(HIQ) Windows */ + + /* spin_lock_bh(&psta_bmc->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + xmitframe_phead = get_list_head(&psta_bmc->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while (xmitframe_phead != xmitframe_plist) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + 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; + + if (xmitframe_hiq_filter(pxmitframe) == true) + pxmitframe->attrib.qsel = 0x11;/* HIQ */ + + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + } + + /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + /* check hi queue and bmc_sleepq */ + rtw_chk_hi_queue_cmd(padapter); + } + + return H2C_SUCCESS; +} + +u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + if (send_beacon(padapter) == _FAIL) { + DBG_871X("issue_beacon, fail!\n"); + return H2C_PARAMETERS_ERROR; + } + + /* tx bc/mc frames after update TIM */ + chk_bmc_sleepq_hdl(padapter, NULL); + + return H2C_SUCCESS; +} + +int rtw_chk_start_clnt_join(struct adapter *padapter, u8 *ch, u8 *bw, u8 *offset) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char cur_ch = pmlmeext->cur_channel; + unsigned char cur_bw = pmlmeext->cur_bwmode; + unsigned char cur_ch_offset = pmlmeext->cur_ch_offset; + bool connect_allow = true; + + if (!ch || !bw || !offset) { + rtw_warn_on(1); + connect_allow = false; + } + + if (connect_allow == true) { + DBG_871X("start_join_set_ch_bw: ch =%d, bwmode =%d, ch_offset =%d\n", cur_ch, cur_bw, cur_ch_offset); + *ch = cur_ch; + *bw = cur_bw; + *offset = cur_ch_offset; + } + + return connect_allow == true ? _SUCCESS : _FAIL; +} + +/* Find union about ch, bw, ch_offset of all linked/linking interfaces */ +int rtw_get_ch_setting_union(struct adapter *adapter, u8 *ch, u8 *bw, u8 *offset) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct adapter *iface; + struct mlme_ext_priv *mlmeext; + u8 ch_ret = 0; + u8 bw_ret = CHANNEL_WIDTH_20; + u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + if (ch) + *ch = 0; + if (bw) + *bw = CHANNEL_WIDTH_20; + if (offset) + *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + iface = dvobj->padapters; + mlmeext = &iface->mlmeextpriv; + + if (!check_fwstate(&iface->mlmepriv, _FW_LINKED|_FW_UNDER_LINKING)) + return 0; + + ch_ret = mlmeext->cur_channel; + bw_ret = mlmeext->cur_bwmode; + offset_ret = mlmeext->cur_ch_offset; + + return 1; +} + +u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf) +{ + struct set_ch_parm *set_ch_parm; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + set_ch_parm = (struct set_ch_parm *)pbuf; + + DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), + set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset); + + pmlmeext->cur_channel = set_ch_parm->ch; + pmlmeext->cur_ch_offset = set_ch_parm->ch_offset; + pmlmeext->cur_bwmode = set_ch_parm->bw; + + set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw); + + return H2C_SUCCESS; +} + +u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + struct SetChannelPlan_param *setChannelPlan_param; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + setChannelPlan_param = (struct SetChannelPlan_param *)pbuf; + + pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set); + init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list); + + if ((padapter->rtw_wdev != NULL) && (padapter->rtw_wdev->wiphy)) { + struct regulatory_request request; + request.initiator = NL80211_REGDOM_SET_BY_DRIVER; + rtw_reg_notifier(padapter->rtw_wdev->wiphy, &request); + } + + return H2C_SUCCESS; +} + +u8 led_blink_hdl(struct 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_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + return H2C_REJECTED; +} + +/* TDLS_ESTABLISHED : write RCR DATA BIT */ +/* 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_TEAR_STA : free tdls sta */ +u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf) +{ + return H2C_REJECTED; +} + +u8 run_in_thread_hdl(struct adapter *padapter, u8 *pbuf) +{ + struct RunInThread_param *p; + + + if (NULL == pbuf) + return H2C_PARAMETERS_ERROR; + p = (struct RunInThread_param *)pbuf; + + if (p->func) + p->func(p->context); + + return H2C_SUCCESS; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_odm.c b/drivers/staging/rtl8723bs/core/rtw_odm.c new file mode 100644 index 0000000..3144e8e --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_odm.c @@ -0,0 +1,197 @@ +/****************************************************************************** + * + * Copyright(c) 2013 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 <drv_types.h> +#include <rtw_debug.h> +#include <rtw_odm.h> +#include <hal_data.h> + +static const char *odm_comp_str[] = { + /* BIT0 */"ODM_COMP_DIG", + /* BIT1 */"ODM_COMP_RA_MASK", + /* BIT2 */"ODM_COMP_DYNAMIC_TXPWR", + /* BIT3 */"ODM_COMP_FA_CNT", + /* BIT4 */"ODM_COMP_RSSI_MONITOR", + /* BIT5 */"ODM_COMP_CCK_PD", + /* BIT6 */"ODM_COMP_ANT_DIV", + /* BIT7 */"ODM_COMP_PWR_SAVE", + /* BIT8 */"ODM_COMP_PWR_TRAIN", + /* BIT9 */"ODM_COMP_RATE_ADAPTIVE", + /* BIT10 */"ODM_COMP_PATH_DIV", + /* BIT11 */"ODM_COMP_PSD", + /* BIT12 */"ODM_COMP_DYNAMIC_PRICCA", + /* BIT13 */"ODM_COMP_RXHP", + /* BIT14 */"ODM_COMP_MP", + /* BIT15 */"ODM_COMP_DYNAMIC_ATC", + /* BIT16 */"ODM_COMP_EDCA_TURBO", + /* BIT17 */"ODM_COMP_EARLY_MODE", + /* BIT18 */NULL, + /* BIT19 */NULL, + /* BIT20 */NULL, + /* BIT21 */NULL, + /* BIT22 */NULL, + /* BIT23 */NULL, + /* BIT24 */"ODM_COMP_TX_PWR_TRACK", + /* BIT25 */"ODM_COMP_RX_GAIN_TRACK", + /* BIT26 */"ODM_COMP_CALIBRATION", + /* BIT27 */NULL, + /* BIT28 */NULL, + /* BIT29 */NULL, + /* BIT30 */"ODM_COMP_COMMON", + /* BIT31 */"ODM_COMP_INIT", +}; + +#define RTW_ODM_COMP_MAX 32 + +static const char *odm_ability_str[] = { + /* BIT0 */"ODM_BB_DIG", + /* BIT1 */"ODM_BB_RA_MASK", + /* BIT2 */"ODM_BB_DYNAMIC_TXPWR", + /* BIT3 */"ODM_BB_FA_CNT", + /* BIT4 */"ODM_BB_RSSI_MONITOR", + /* BIT5 */"ODM_BB_CCK_PD", + /* BIT6 */"ODM_BB_ANT_DIV", + /* BIT7 */"ODM_BB_PWR_SAVE", + /* BIT8 */"ODM_BB_PWR_TRAIN", + /* BIT9 */"ODM_BB_RATE_ADAPTIVE", + /* BIT10 */"ODM_BB_PATH_DIV", + /* BIT11 */"ODM_BB_PSD", + /* BIT12 */"ODM_BB_RXHP", + /* BIT13 */"ODM_BB_ADAPTIVITY", + /* BIT14 */"ODM_BB_DYNAMIC_ATC", + /* BIT15 */NULL, + /* BIT16 */"ODM_MAC_EDCA_TURBO", + /* BIT17 */"ODM_MAC_EARLY_MODE", + /* BIT18 */NULL, + /* BIT19 */NULL, + /* BIT20 */NULL, + /* BIT21 */NULL, + /* BIT22 */NULL, + /* BIT23 */NULL, + /* BIT24 */"ODM_RF_TX_PWR_TRACK", + /* BIT25 */"ODM_RF_RX_GAIN_TRACK", + /* BIT26 */"ODM_RF_CALIBRATION", +}; + +#define RTW_ODM_ABILITY_MAX 27 + +static const char *odm_dbg_level_str[] = { + NULL, + "ODM_DBG_OFF", + "ODM_DBG_SERIOUS", + "ODM_DBG_WARNING", + "ODM_DBG_LOUD", + "ODM_DBG_TRACE", +}; + +#define RTW_ODM_DBG_LEVEL_NUM 6 + +void rtw_odm_dbg_comp_msg(void *sel, struct adapter *adapter) +{ + u64 dbg_comp; + int i; + + rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &dbg_comp); + DBG_871X_SEL_NL(sel, "odm.DebugComponents = 0x%016llx\n", dbg_comp); + for (i = 0; i < RTW_ODM_COMP_MAX; i++) { + if (odm_comp_str[i]) + DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n", + (BIT0 << i) & dbg_comp ? '+' : ' ', + i, odm_comp_str[i]); + } +} + +inline void rtw_odm_dbg_comp_set(struct adapter *adapter, u64 comps) +{ + rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &comps); +} + +void rtw_odm_dbg_level_msg(void *sel, struct adapter *adapter) +{ + u32 dbg_level; + int i; + + rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &dbg_level); + DBG_871X_SEL_NL(sel, "odm.DebugLevel = %u\n", dbg_level); + for (i = 0; i < RTW_ODM_DBG_LEVEL_NUM; i++) { + if (odm_dbg_level_str[i]) + DBG_871X_SEL_NL(sel, "%u %s\n", i, odm_dbg_level_str[i]); + } +} + +inline void rtw_odm_dbg_level_set(struct adapter *adapter, u32 level) +{ + rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &level); +} + +void rtw_odm_ability_msg(void *sel, struct adapter *adapter) +{ + u32 ability = 0; + int i; + + rtw_hal_get_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability); + DBG_871X_SEL_NL(sel, "odm.SupportAbility = 0x%08x\n", ability); + for (i = 0; i < RTW_ODM_ABILITY_MAX; i++) { + if (odm_ability_str[i]) + DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n", + (BIT0 << i) & ability ? '+' : ' ', i, + odm_ability_str[i]); + } +} + +inline void rtw_odm_ability_set(struct adapter *adapter, u32 ability) +{ + rtw_hal_set_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability); +} + +void rtw_odm_adaptivity_parm_msg(void *sel, struct adapter *adapter) +{ + struct hal_com_data *pHalData = GET_HAL_DATA(adapter); + DM_ODM_T *odm = &pHalData->odmpriv; + + DBG_871X_SEL_NL(sel, "%10s %16s %8s %10s %11s %14s\n" + , "TH_L2H_ini", "TH_EDCCA_HL_diff", "IGI_Base", "ForceEDCCA", "AdapEn_RSSI", "IGI_LowerBound"); + DBG_871X_SEL_NL(sel, "0x%-8x %-16d 0x%-6x %-10d %-11u %-14u\n" + , (u8)odm->TH_L2H_ini + , odm->TH_EDCCA_HL_diff + , odm->IGI_Base + , odm->ForceEDCCA + , odm->AdapEn_RSSI + , odm->IGI_LowerBound + ); +} + +void rtw_odm_adaptivity_parm_set(struct adapter *adapter, s8 TH_L2H_ini, s8 TH_EDCCA_HL_diff, + s8 IGI_Base, bool ForceEDCCA, u8 AdapEn_RSSI, u8 IGI_LowerBound) +{ + struct hal_com_data *pHalData = GET_HAL_DATA(adapter); + DM_ODM_T *odm = &pHalData->odmpriv; + + odm->TH_L2H_ini = TH_L2H_ini; + odm->TH_EDCCA_HL_diff = TH_EDCCA_HL_diff; + odm->IGI_Base = IGI_Base; + odm->ForceEDCCA = ForceEDCCA; + odm->AdapEn_RSSI = AdapEn_RSSI; + odm->IGI_LowerBound = IGI_LowerBound; +} + +void rtw_odm_get_perpkt_rssi(void *sel, struct adapter *adapter) +{ + struct hal_com_data *hal_data = GET_HAL_DATA(adapter); + DM_ODM_T *odm = &(hal_data->odmpriv); + + DBG_871X_SEL_NL(sel, "RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n", + HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B); +} diff --git a/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c new file mode 100644 index 0000000..f708dbf --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c @@ -0,0 +1,1421 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <hal_data.h> +#include <linux/jiffies.h> + + +void _ips_enter(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + pwrpriv->bips_processing = true; + + /* syn ips_mode with request */ + pwrpriv->ips_mode = pwrpriv->ips_mode_req; + + pwrpriv->ips_enter_cnts++; + DBG_871X("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts); + + if (rf_off == pwrpriv->change_rfpwrstate) { + pwrpriv->bpower_saving = true; + DBG_871X_LEVEL(_drv_always_, "nolinked power save enter\n"); + + if (pwrpriv->ips_mode == IPS_LEVEL_2) + pwrpriv->bkeepfwalive = true; + + rtw_ips_pwr_down(padapter); + pwrpriv->rf_pwrstate = rf_off; + } + pwrpriv->bips_processing = false; + +} + +void ips_enter(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + + rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req); + + down(&pwrpriv->lock); + _ips_enter(padapter); + up(&pwrpriv->lock); +} + +int _ips_leave(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + int result = _SUCCESS; + + if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) { + pwrpriv->bips_processing = true; + pwrpriv->change_rfpwrstate = rf_on; + pwrpriv->ips_leave_cnts++; + DBG_871X("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts); + + result = rtw_ips_pwr_up(padapter); + if (result == _SUCCESS) { + pwrpriv->rf_pwrstate = rf_on; + } + DBG_871X_LEVEL(_drv_always_, "nolinked power save leave\n"); + + DBG_871X("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c)); + pwrpriv->bips_processing = false; + + pwrpriv->bkeepfwalive = false; + pwrpriv->bpower_saving = false; + } + + return result; +} + +int ips_leave(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + int ret; + + if (!is_primary_adapter(padapter)) + return _SUCCESS; + + down(&pwrpriv->lock); + ret = _ips_leave(padapter); + up(&pwrpriv->lock); + + if (_SUCCESS == ret) + rtw_btcoex_IpsNotify(padapter, IPS_NONE); + + return ret; +} + +static bool rtw_pwr_unassociated_idle(struct adapter *adapter) +{ + struct adapter *buddy = adapter->pbuddy_adapter; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct xmit_priv *pxmit_priv = &adapter->xmitpriv; + + bool ret = false; + + if (adapter_to_pwrctl(adapter)->bpower_saving == true) { + /* DBG_871X("%s: already in LPS or IPS mode\n", __func__); */ + goto exit; + } + + if (time_before(jiffies, adapter_to_pwrctl(adapter)->ips_deny_time)) { + /* DBG_871X("%s ips_deny_time\n", __func__); */ + 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) + ) + goto exit; + + /* consider buddy, if exist */ + if (buddy) { + struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv); + + 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) + ) + goto exit; + } + + if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF || + pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) { + DBG_871X_LEVEL(_drv_always_, "There are some pkts to transmit\n"); + DBG_871X_LEVEL(_drv_always_, "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; +} + + +/* + * ATTENTION: + *rtw_ps_processor() doesn't handle LPS. + */ +void rtw_ps_processor(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + u32 ps_deny = 0; + + down(&adapter_to_pwrctl(padapter)->lock); + ps_deny = rtw_ps_deny_get(padapter); + up(&adapter_to_pwrctl(padapter)->lock); + if (ps_deny != 0) { + DBG_871X(FUNC_ADPT_FMT ": ps_deny = 0x%08X, skip power save!\n", + FUNC_ADPT_ARG(padapter), ps_deny); + goto exit; + } + + if (pwrpriv->bInSuspend == true) {/* system suspend or autosuspend */ + pdbgpriv->dbg_ps_insuspend_cnt++; + DBG_871X("%s, pwrpriv->bInSuspend == true ignore this process\n", __func__); + return; + } + + pwrpriv->ps_processing = true; + + 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_871X("==>%s\n", __func__); + pwrpriv->change_rfpwrstate = rf_off; + { + ips_enter(padapter); + } + } +exit: + pwrpriv->ps_processing = false; + return; +} + +void pwr_state_check_handler(RTW_TIMER_HDL_ARGS); +void pwr_state_check_handler(RTW_TIMER_HDL_ARGS) +{ + struct adapter *padapter = (struct adapter *)FunctionContext; + rtw_ps_cmd(padapter); +} + +void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets) +{ + static unsigned long start_time = 0; + static u32 xmit_cnt = 0; + u8 bLeaveLPS = false; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + + if (tx) { /* from tx */ + xmit_cnt += tx_packets; + + if (start_time == 0) + start_time = jiffies; + + if (jiffies_to_msecs(jiffies - start_time) > 2000) { /* 2 sec == watch dog timer */ + if (xmit_cnt > 8) { + if ((adapter_to_pwrctl(padapter)->bLeisurePs) + && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE) + && (rtw_btcoex_IsBtControlLps(padapter) == false) + ) { + DBG_871X("leave lps via Tx = %d\n", xmit_cnt); + bLeaveLPS = true; + } + } + + start_time = jiffies; + xmit_cnt = 0; + } + + } else { /* from rx path */ + if (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4/*2*/) { + if ((adapter_to_pwrctl(padapter)->bLeisurePs) + && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE) + && (rtw_btcoex_IsBtControlLps(padapter) == false) + ) { + DBG_871X("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); + bLeaveLPS = true; + } + } + } + + if (bLeaveLPS) + /* DBG_871X("leave lps via %s, Tx = %d, Rx = %d\n", tx?"Tx":"Rx", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */ + /* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1); */ + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, tx?0:1); +} + +/* + * Description: + *This function MUST be called under power lock protect + * + * Parameters + *padapter + *pslv power state level, only could be PS_STATE_S0 ~ PS_STATE_S4 + * + */ +void rtw_set_rpwm(struct adapter *padapter, u8 pslv) +{ + u8 rpwm; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 cpwm_orig; + + pslv = PS_STATE(pslv); + + if (pwrpriv->brpwmtimeout == true) { + DBG_871X("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __func__, pslv); + } else{ + if ((pwrpriv->rpwm == pslv) + || ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2))) { + 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; + /* only when from PS_STATE S0/S1 to S2 and higher needs ACK */ + if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2)) + rpwm |= PS_ACK; + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("rtw_set_rpwm: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm)); + + pwrpriv->rpwm = pslv; + + cpwm_orig = 0; + if (rpwm & PS_ACK) + rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig); + + if (rpwm & PS_ACK) + _set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS); + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm)); + + pwrpriv->tog += 0x80; + + /* No LPS 32K, No Ack */ + if (rpwm & PS_ACK) { + unsigned long start_time; + u8 cpwm_now; + u8 poll_cnt = 0; + + start_time = jiffies; + + /* polling cpwm */ + do { + mdelay(1); + poll_cnt++; + rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now); + if ((cpwm_orig ^ cpwm_now) & 0x80) { + pwrpriv->cpwm = PS_STATE_S4; + pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE; + break; + } + + if (jiffies_to_msecs(jiffies - start_time) > LPS_RPWM_WAIT_MS) { + DBG_871X("%s: polling cpwm timeout! poll_cnt =%d, cpwm_orig =%02x, cpwm_now =%02x\n", __func__, poll_cnt, cpwm_orig, cpwm_now); + _set_timer(&pwrpriv->pwr_rpwm_timer, 1); + break; + } + } while (1); + } else + pwrpriv->cpwm = pslv; +} + +static u8 PS_RDY_CHECK(struct adapter *padapter) +{ + unsigned long curr_time, delta_time; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) + if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_mode) + return true; + else if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode) + return true; + else if (true == pwrpriv->bInSuspend) + return false; +#else + if (true == pwrpriv->bInSuspend) + return false; +#endif + + curr_time = jiffies; + + delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp; + + if (delta_time < LPS_DELAY_TIME) + return false; + + if (check_fwstate(pmlmepriv, 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_is_scan_deny(padapter) + ) + return false; + + if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false)) { + DBG_871X("Group handshake still in progress !!!\n"); + return false; + } + + if (!rtw_cfg80211_pwr_mgmt(padapter)) + return false; + + return true; +} + +void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) + struct debug_priv *pdbgpriv = &padapter->dvobj->drv_dbg; +#endif + + 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; + + + down(&pwrpriv->lock); + + /* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */ + if (ps_mode == PS_MODE_ACTIVE) { + if (1 + && (((rtw_btcoex_IsBtControlLps(padapter) == false) + ) + || ((rtw_btcoex_IsBtControlLps(padapter) == true) + && (rtw_btcoex_IsLpsOn(padapter) == false)) + ) + ) { + DBG_871X(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n", + FUNC_ADPT_ARG(padapter), msg); + + pwrpriv->pwr_mode = ps_mode; + rtw_set_rpwm(padapter, PS_STATE_S4); + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) + if (pwrpriv->wowlan_mode == true || + pwrpriv->wowlan_ap_mode == true) { + unsigned long start_time; + u32 delay_ms; + u8 val8; + delay_ms = 20; + start_time = jiffies; + do { + rtw_hal_get_hwreg(padapter, HW_VAR_SYS_CLKR, &val8); + if (!(val8 & BIT(4))) { /* 0x08 bit4 = 1 --> in 32k, bit4 = 0 --> leave 32k */ + pwrpriv->cpwm = PS_STATE_S4; + break; + } + if (jiffies_to_msecs(jiffies - start_time) > delay_ms) { + DBG_871X("%s: Wait for FW 32K leave more than %u ms!!!\n", + __func__, delay_ms); + pdbgpriv->dbg_wow_leave_ps_fail_cnt++; + break; + } + msleep(1); + } while (1); + } +#endif + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + pwrpriv->bFwCurrentInPSMode = false; + + rtw_btcoex_LpsNotify(padapter, ps_mode); + } + } else{ + if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE)) + || ((rtw_btcoex_IsBtControlLps(padapter) == true) + && (rtw_btcoex_IsLpsOn(padapter) == true)) + ) { + u8 pslv; + + DBG_871X(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n", + FUNC_ADPT_ARG(padapter), msg); + + rtw_btcoex_LpsNotify(padapter, ps_mode); + + pwrpriv->bFwCurrentInPSMode = true; + pwrpriv->pwr_mode = ps_mode; + pwrpriv->smart_ps = smart_ps; + pwrpriv->bcn_ant_mode = bcn_ant_mode; + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + + pslv = PS_STATE_S2; + if (pwrpriv->alives == 0) + pslv = PS_STATE_S0; + + if ((rtw_btcoex_IsBtDisabled(padapter) == false) + && (rtw_btcoex_IsBtControlLps(padapter) == true)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + } + + rtw_set_rpwm(padapter, pslv); + } + } + + up(&pwrpriv->lock); +} + +/* + * Return: + *0: Leave OK + *-1: Timeout + *-2: Other error + */ +s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms) +{ + unsigned long start_time; + u8 bAwake = false; + s32 err = 0; + + + start_time = jiffies; + while (1) { + rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake); + if (true == bAwake) + break; + + if (true == padapter->bSurpriseRemoved) { + err = -2; + DBG_871X("%s: device surprise removed!!\n", __func__); + break; + } + + if (jiffies_to_msecs(jiffies - start_time) > delay_ms) { + err = -1; + DBG_871X("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms); + break; + } + msleep(1); + } + + return err; +} + +/* */ +/* Description: */ +/* Enter the leisure power save mode. */ +/* */ +void LPS_Enter(struct adapter *padapter, const char *msg) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + int n_assoc_iface = 0; + char buf[32] = {0}; + + if (rtw_btcoex_IsBtControlLps(padapter) == true) + return; + + /* Skip lps enter request if number of assocated adapters is not 1 */ + if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE)) + n_assoc_iface++; + if (n_assoc_iface != 1) + return; + + /* Skip lps enter request for adapter not port0 */ + if (get_iface_type(padapter) != IFACE_PORT0) + return; + + if (PS_RDY_CHECK(dvobj->padapters) == false) + return; + + if (pwrpriv->bLeisurePs) { + /* Idle for a while if we connect to AP a while ago. */ + if (pwrpriv->LpsIdleCount >= 2) { /* 4 Sec */ + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) { + sprintf(buf, "WIFI-%s", msg); + pwrpriv->bpower_saving = true; + rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, padapter->registrypriv.smart_ps, 0, buf); + } + } else + pwrpriv->LpsIdleCount++; + } + +/* DBG_871X("-LeisurePSEnter\n"); */ +} + +/* */ +/* Description: */ +/* Leave the leisure power save mode. */ +/* */ +void LPS_Leave(struct adapter *padapter, const char *msg) +{ +#define LPS_LEAVE_TIMEOUT_MS 100 + + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char buf[32] = {0}; + +/* DBG_871X("+LeisurePSLeave\n"); */ + + if (rtw_btcoex_IsBtControlLps(padapter) == true) + return; + + if (pwrpriv->bLeisurePs) { + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + sprintf(buf, "WIFI-%s", msg); + rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, buf); + + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) + LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS); + } + } + + pwrpriv->bpower_saving = false; +/* DBG_871X("-LeisurePSLeave\n"); */ + +} + +void LeaveAllPowerSaveModeDirect(struct adapter *Adapter) +{ + struct adapter *pri_padapter = GET_PRIMARY_ADAPTER(Adapter); + struct mlme_priv *pmlmepriv = &(Adapter->mlmepriv); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter); + + DBG_871X("%s.....\n", __func__); + + if (true == Adapter->bSurpriseRemoved) { + DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n", + FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved); + return; + } + + if ((check_fwstate(pmlmepriv, _FW_LINKED) == true)) { /* connect */ + + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) { + DBG_871X("%s: Driver Already Leave LPS\n", __func__); + return; + } + + down(&pwrpriv->lock); + + rtw_set_rpwm(Adapter, PS_STATE_S4); + + up(&pwrpriv->lock); + + rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0); + } else{ + if (pwrpriv->rf_pwrstate == rf_off) + if (false == ips_leave(pri_padapter)) + DBG_871X("======> ips_leave fail.............\n"); + } +} + +/* */ +/* Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */ +/* Move code to function by tynli. 2010.03.26. */ +/* */ +void LeaveAllPowerSaveMode(struct adapter *Adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter); + u8 enqueue = 0; + int n_assoc_iface = 0; + + if (!Adapter->bup) { + DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n", + FUNC_ADPT_ARG(Adapter), Adapter->bup); + return; + } + + if (Adapter->bSurpriseRemoved) { + DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n", + FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved); + return; + } + + if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE)) + n_assoc_iface++; + + if (n_assoc_iface) { /* connect */ + enqueue = 1; + + rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue); + + LPS_Leave_check(Adapter); + } else { + if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) { + if (false == ips_leave(Adapter)) + DBG_871X("======> ips_leave fail.............\n"); + } + } +} + +void LPS_Leave_check( + struct adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv; + unsigned long start_time; + u8 bReady; + + pwrpriv = adapter_to_pwrctl(padapter); + + bReady = false; + start_time = jiffies; + + yield(); + + while (1) { + down(&pwrpriv->lock); + + if ((padapter->bSurpriseRemoved == true) + || (padapter->hw_init_completed == false) + || (pwrpriv->pwr_mode == PS_MODE_ACTIVE) + ) + bReady = true; + + up(&pwrpriv->lock); + + if (true == bReady) + break; + + if (jiffies_to_msecs(jiffies - start_time) > 100) { + DBG_871X("Wait for cpwm event than 100 ms!!!\n"); + break; + } + msleep(1); + } +} + +/* + * Caller:ISR handler... + * + * This will be called when CPWM interrupt is up. + * + * using to update cpwn of drv; and drv willl make a decision to up or down pwr level + */ +void cpwm_int_hdl( + struct adapter *padapter, + struct reportpwrstate_parm *preportpwrstate) +{ + struct pwrctrl_priv *pwrpriv; + + pwrpriv = adapter_to_pwrctl(padapter); + + down(&pwrpriv->lock); + + if (pwrpriv->rpwm < PS_STATE_S2) { + DBG_871X("%s: Redundant CPWM Int. RPWM = 0x%02X CPWM = 0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + up(&pwrpriv->lock); + goto exit; + } + + pwrpriv->cpwm = PS_STATE(preportpwrstate->state); + pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE; + + if (pwrpriv->cpwm >= PS_STATE_S2) { + if (pwrpriv->alives & CMD_ALIVE) + up(&padapter->cmdpriv.cmd_queue_sema); + + if (pwrpriv->alives & XMIT_ALIVE) + up(&padapter->xmitpriv.xmit_sema); + } + + up(&pwrpriv->lock); + +exit: + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("cpwm_int_hdl: cpwm = 0x%02x\n", pwrpriv->cpwm)); +} + +static void cpwm_event_callback(struct work_struct *work) +{ + struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + struct adapter *adapter = dvobj->if1; + struct reportpwrstate_parm report; + + /* DBG_871X("%s\n", __func__); */ + + report.state = PS_STATE_S2; + cpwm_int_hdl(adapter, &report); +} + +static void rpwmtimeout_workitem_callback(struct work_struct *work) +{ + struct adapter *padapter; + struct dvobj_priv *dvobj; + struct pwrctrl_priv *pwrpriv; + + + pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi); + dvobj = pwrctl_to_dvobj(pwrpriv); + padapter = dvobj->if1; +/* DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); */ + + down(&pwrpriv->lock); + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + DBG_871X("%s: rpwm = 0x%02X cpwm = 0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + goto exit; + } + up(&pwrpriv->lock); + + if (rtw_read8(padapter, 0x100) != 0xEA) { + struct reportpwrstate_parm report; + + report.state = PS_STATE_S2; + DBG_871X("\n%s: FW already leave 32K!\n\n", __func__); + cpwm_int_hdl(padapter, &report); + + return; + } + + down(&pwrpriv->lock); + + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + DBG_871X("%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm); + goto exit; + } + pwrpriv->brpwmtimeout = true; + rtw_set_rpwm(padapter, pwrpriv->rpwm); + pwrpriv->brpwmtimeout = false; + +exit: + up(&pwrpriv->lock); +} + +/* + * This function is a timer handler, can't do any IO in it. + */ +static void pwr_rpwm_timeout_handler(void *FunctionContext) +{ + struct adapter *padapter; + struct pwrctrl_priv *pwrpriv; + + + padapter = (struct adapter *)FunctionContext; + pwrpriv = adapter_to_pwrctl(padapter); + DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + DBG_871X("+%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm); + return; + } + + _set_workitem(&pwrpriv->rpwmtimeoutwi); +} + +static __inline void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag) +{ + pwrctrl->alives |= tag; +} + +static __inline void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag) +{ + pwrctrl->alives &= ~tag; +} + + +/* + * Description: + *Check if the fw_pwrstate is okay for I/O. + *If not (cpwm is less than S2), then the sub-routine + *will raise the cpwm to be greater than or equal to S2. + * + *Calling Context: Passive + * + *Constraint: + * 1. this function will request pwrctrl->lock + * + * Return Value: + *_SUCCESS hardware is ready for I/O + *_FAIL can't I/O right now + */ +s32 rtw_register_task_alive(struct adapter *padapter, u32 task) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + down(&pwrctrl->lock); + + register_task_alive(pwrctrl, task); + + if (pwrctrl->bFwCurrentInPSMode == true) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("%s: task = 0x%x cpwm = 0x%02x alives = 0x%08x\n", + __func__, task, pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + up(&pwrctrl->lock); + + if (_FAIL == res) + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + + return res; +} + +/* + * Description: + *If task is done, call this func. to power down firmware again. + * + *Constraint: + * 1. this function will request pwrctrl->lock + * + * Return Value: + *none + */ +void rtw_unregister_task_alive(struct adapter *padapter, u32 task) +{ + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + + if ((rtw_btcoex_IsBtDisabled(padapter) == false) + && (rtw_btcoex_IsBtControlLps(padapter) == true)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + } + + down(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, task); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == true)) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("%s: cpwm = 0x%02x alives = 0x%08x\n", + __func__, pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm > pslv) + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + + } + + up(&pwrctrl->lock); +} + +/* + * Caller: rtw_xmit_thread + * + * Check if the fw_pwrstate is okay for xmit. + * If not (cpwm is less than S3), then the sub-routine + * will raise the cpwm to be greater than or equal to S3. + * + * Calling Context: Passive + * + * Return Value: + * _SUCCESS rtw_xmit_thread can write fifo/txcmd afterwards. + * _FAIL rtw_xmit_thread can not do anything. + */ +s32 rtw_register_tx_alive(struct adapter *padapter) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + down(&pwrctrl->lock); + + register_task_alive(pwrctrl, XMIT_ALIVE); + + if (pwrctrl->bFwCurrentInPSMode == true) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("rtw_register_tx_alive: cpwm = 0x%02x alives = 0x%08x\n", + pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + up(&pwrctrl->lock); + + if (_FAIL == res) + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + + return res; +} + +/* + * Caller: rtw_cmd_thread + * + * Check if the fw_pwrstate is okay for issuing cmd. + * If not (cpwm should be is less than S2), then the sub-routine + * will raise the cpwm to be greater than or equal to S2. + * + * Calling Context: Passive + * + * Return Value: + *_SUCCESS rtw_cmd_thread can issue cmds to firmware afterwards. + *_FAIL rtw_cmd_thread can not do anything. + */ +s32 rtw_register_cmd_alive(struct adapter *padapter) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + down(&pwrctrl->lock); + + register_task_alive(pwrctrl, CMD_ALIVE); + + if (pwrctrl->bFwCurrentInPSMode == true) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_, + ("rtw_register_cmd_alive: cpwm = 0x%02x alives = 0x%08x\n", + pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + up(&pwrctrl->lock); + + if (_FAIL == res) + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + + return res; +} + +/* + * Caller: ISR + * + * If ISR's txdone, + * No more pkts for TX, + * Then driver shall call this fun. to power down firmware again. + */ +void rtw_unregister_tx_alive(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + + if ((rtw_btcoex_IsBtDisabled(padapter) == false) + && (rtw_btcoex_IsBtControlLps(padapter) == true)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + } + + down(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, XMIT_ALIVE); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == true)) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_, + ("%s: cpwm = 0x%02x alives = 0x%08x\n", + __func__, pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm > pslv) + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + } + + up(&pwrctrl->lock); +} + +/* + * Caller: ISR + * + * If all commands have been done, + * and no more command to do, + * then driver shall call this fun. to power down firmware again. + */ +void rtw_unregister_cmd_alive(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + + if ((rtw_btcoex_IsBtDisabled(padapter) == false) + && (rtw_btcoex_IsBtControlLps(padapter) == true)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + } + + down(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, CMD_ALIVE); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == true)) { + RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_, + ("%s: cpwm = 0x%02x alives = 0x%08x\n", + __func__, pwrctrl->cpwm, pwrctrl->alives)); + + if (pwrctrl->cpwm > pslv) { + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + } + } + + up(&pwrctrl->lock); +} + +void rtw_init_pwrctrl_priv(struct adapter *padapter) +{ + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + sema_init(&pwrctrlpriv->lock, 1); + sema_init(&pwrctrlpriv->check_32k_lock, 1); + pwrctrlpriv->rf_pwrstate = rf_on; + pwrctrlpriv->ips_enter_cnts = 0; + pwrctrlpriv->ips_leave_cnts = 0; + pwrctrlpriv->bips_processing = false; + + pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode; + pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode; + + pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL; + pwrctrlpriv->pwr_state_check_cnts = 0; + pwrctrlpriv->bInternalAutoSuspend = false; + pwrctrlpriv->bInSuspend = false; + pwrctrlpriv->bkeepfwalive = false; + + pwrctrlpriv->LpsIdleCount = 0; + 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->dtim = 0; + + pwrctrlpriv->tog = 0x80; + + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm)); + + _init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL); + + pwrctrlpriv->brpwmtimeout = false; + _init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL); + _init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter->pnetdev, pwr_rpwm_timeout_handler, padapter); + + rtw_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter, pwr_state_check_handler); + + pwrctrlpriv->wowlan_mode = false; + pwrctrlpriv->wowlan_ap_mode = false; + +#ifdef CONFIG_PNO_SUPPORT + pwrctrlpriv->pno_inited = false; + pwrctrlpriv->pnlo_info = NULL; + pwrctrlpriv->pscan_info = NULL; + pwrctrlpriv->pno_ssid_list = NULL; + pwrctrlpriv->pno_in_resume = true; +#endif +} + + +void rtw_free_pwrctrl_priv(struct adapter *adapter) +{ + /* memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv)); */ + +#ifdef CONFIG_PNO_SUPPORT + if (pwrctrlpriv->pnlo_info != NULL) + printk("****** pnlo_info memory leak********\n"); + + if (pwrctrlpriv->pscan_info != NULL) + printk("****** pscan_info memory leak********\n"); + + if (pwrctrlpriv->pno_ssid_list != NULL) + printk("****** pno_ssid_list memory leak********\n"); +#endif +} + +inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ms); +} + +/* +* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend +* @adapter: pointer to struct adapter structure +* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup +* Return _SUCCESS or _FAIL +*/ + +int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + struct mlme_priv *pmlmepriv; + int ret = _SUCCESS; + unsigned long start = jiffies; + unsigned long deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms); + + /* for LPS */ + LeaveAllPowerSaveMode(padapter); + + /* IPS still bound with primary adapter */ + padapter = GET_PRIMARY_ADAPTER(padapter); + pmlmepriv = &padapter->mlmepriv; + + if (time_before(pwrpriv->ips_deny_time, deny_time)) + pwrpriv->ips_deny_time = deny_time; + + + if (pwrpriv->ps_processing) { + DBG_871X("%s wait ps_processing...\n", __func__); + while (pwrpriv->ps_processing && jiffies_to_msecs(jiffies - start) <= 3000) + msleep(10); + if (pwrpriv->ps_processing) + DBG_871X("%s wait ps_processing timeout\n", __func__); + else + DBG_871X("%s wait ps_processing done\n", __func__); + } + + if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) { + DBG_871X("%s wait bInSuspend...\n", __func__); + while (pwrpriv->bInSuspend + && jiffies_to_msecs(jiffies - start) <= 3000 + ) { + msleep(10); + } + if (pwrpriv->bInSuspend) + DBG_871X("%s wait bInSuspend timeout\n", __func__); + else + DBG_871X("%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_8192C("%s call ips_leave....\n", __func__); + if (_FAIL == ips_leave(padapter)) { + DBG_8192C("======> ips_leave fail.............\n"); + ret = _FAIL; + goto exit; + } + } + } + + /* TODO: the following checking need to be merged... */ + if (padapter->bDriverStopped + || !padapter->bup + || !padapter->hw_init_completed + ) { + DBG_8192C("%s: bDriverStopped =%d, bup =%d, hw_init_completed =%u\n" + , caller + , padapter->bDriverStopped + , padapter->bup + , padapter->hw_init_completed); + ret = false; + goto exit; + } + +exit: + deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms); + if (time_before(pwrpriv->ips_deny_time, deny_time)) + pwrpriv->ips_deny_time = deny_time; + return ret; + +} + +int rtw_pm_set_lps(struct adapter *padapter, u8 mode) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (mode < PS_MODE_NUM) { + if (pwrctrlpriv->power_mgnt != mode) { + if (PS_MODE_ACTIVE == mode) + LeaveAllPowerSaveMode(padapter); + else + pwrctrlpriv->LpsIdleCount = 2; + + pwrctrlpriv->power_mgnt = mode; + pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false; + } + } else + ret = -EINVAL; + + return ret; +} + +int rtw_pm_set_ips(struct adapter *padapter, u8 mode) +{ + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) { + rtw_ips_mode_req(pwrctrlpriv, mode); + DBG_871X("%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_871X("%s %s\n", __func__, "IPS_NONE"); + if ((padapter->bSurpriseRemoved == 0) && (_FAIL == rtw_pwr_wakeup(padapter))) + return -EFAULT; + } else + return -EINVAL; + + return 0; +} + +/* + * ATTENTION: + *This function will request pwrctrl LOCK! + */ +void rtw_ps_deny(struct adapter *padapter, enum PS_DENY_REASON reason) +{ + struct pwrctrl_priv *pwrpriv; + +/* DBG_871X("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n", */ +/* FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */ + + pwrpriv = adapter_to_pwrctl(padapter); + + down(&pwrpriv->lock); + if (pwrpriv->ps_deny & BIT(reason)) { + DBG_871X(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n", + FUNC_ADPT_ARG(padapter), reason); + } + pwrpriv->ps_deny |= BIT(reason); + up(&pwrpriv->lock); + +/* DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */ +/* FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */ +} + +/* + * ATTENTION: + *This function will request pwrctrl LOCK! + */ +void rtw_ps_deny_cancel(struct adapter *padapter, enum PS_DENY_REASON reason) +{ + struct pwrctrl_priv *pwrpriv; + + +/* DBG_871X("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n", */ +/* FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */ + + pwrpriv = adapter_to_pwrctl(padapter); + + down(&pwrpriv->lock); + if ((pwrpriv->ps_deny & BIT(reason)) == 0) { + DBG_871X(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n", + FUNC_ADPT_ARG(padapter), reason); + } + pwrpriv->ps_deny &= ~BIT(reason); + up(&pwrpriv->lock); + +/* DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */ +/* FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */ +} + +/* + * ATTENTION: + *Before calling this function pwrctrl lock should be occupied already, + *otherwise it may return incorrect value. + */ +u32 rtw_ps_deny_get(struct adapter *padapter) +{ + u32 deny; + + + deny = adapter_to_pwrctl(padapter)->ps_deny; + + return deny; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_recv.c b/drivers/staging/rtl8723bs/core/rtw_recv.c new file mode 100644 index 0000000..695a5c9 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_recv.c @@ -0,0 +1,2689 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <linux/jiffies.h> +#include <rtw_recv.h> + +static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37}; +static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3}; + +u8 rtw_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 }; +/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */ +u8 rtw_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 }; + +void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS); + +void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv) +{ + memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv)); + + spin_lock_init(&psta_recvpriv->lock); + + /* for (i = 0; i<MAX_RX_NUMBLKS; i++) */ + /* _rtw_init_queue(&psta_recvpriv->blk_strms[i]); */ + + _rtw_init_queue(&psta_recvpriv->defrag_q); +} + +sint _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter) +{ + sint i; + union recv_frame *precvframe; + sint res = _SUCCESS; + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */ + /* memset((unsigned char *)precvpriv, 0, sizeof (struct recv_priv)); */ + + spin_lock_init(&precvpriv->lock); + + _rtw_init_queue(&precvpriv->free_recv_queue); + _rtw_init_queue(&precvpriv->recv_pending_queue); + _rtw_init_queue(&precvpriv->uc_swdec_pending_queue); + + precvpriv->adapter = padapter; + + precvpriv->free_recvframe_cnt = NR_RECVFRAME; + + precvpriv->pallocated_frame_buf = vzalloc(NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); + + if (precvpriv->pallocated_frame_buf == NULL) { + res = _FAIL; + goto exit; + } + /* memset(precvpriv->pallocated_frame_buf, 0, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); */ + + precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ); + /* precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf + RXFRAME_ALIGN_SZ - */ + /* ((SIZE_PTR) (precvpriv->pallocated_frame_buf) &(RXFRAME_ALIGN_SZ-1)); */ + + precvframe = (union recv_frame *) precvpriv->precv_frame_buf; + + + for (i = 0; i < NR_RECVFRAME; i++) { + INIT_LIST_HEAD(&(precvframe->u.list)); + + list_add_tail(&(precvframe->u.list), &(precvpriv->free_recv_queue.queue)); + + res = rtw_os_recv_resource_alloc(padapter, precvframe); + + precvframe->u.hdr.len = 0; + + precvframe->u.hdr.adapter = padapter; + precvframe++; + + } + + res = rtw_hal_init_recv_priv(padapter); + + rtw_init_timer(&precvpriv->signal_stat_timer, padapter, rtw_signal_stat_timer_hdl); + + precvpriv->signal_stat_sampling_interval = 2000; /* ms */ + + rtw_set_signal_stat_timer(precvpriv); + +exit: + return res; +} + +void _rtw_free_recv_priv(struct recv_priv *precvpriv) +{ + struct adapter *padapter = precvpriv->adapter; + + rtw_free_uc_swdec_pending_queue(padapter); + + rtw_os_recv_resource_free(precvpriv); + + if (precvpriv->pallocated_frame_buf) + vfree(precvpriv->pallocated_frame_buf); + + rtw_hal_free_recv_priv(padapter); +} + +union recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue) +{ + + union recv_frame *precvframe; + struct list_head *plist, *phead; + struct adapter *padapter; + struct recv_priv *precvpriv; + + if (list_empty(&pfree_recv_queue->queue)) + precvframe = NULL; + else{ + phead = get_list_head(pfree_recv_queue); + + plist = get_next(phead); + + precvframe = LIST_CONTAINOR(plist, union recv_frame, u); + + list_del_init(&precvframe->u.hdr.list); + padapter = precvframe->u.hdr.adapter; + if (padapter != NULL) { + precvpriv = &padapter->recvpriv; + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt--; + } + } + return precvframe; +} + +union recv_frame *rtw_alloc_recvframe(struct __queue *pfree_recv_queue) +{ + union recv_frame *precvframe; + + spin_lock_bh(&pfree_recv_queue->lock); + + precvframe = _rtw_alloc_recvframe(pfree_recv_queue); + + spin_unlock_bh(&pfree_recv_queue->lock); + + return precvframe; +} + +int rtw_free_recvframe(union recv_frame *precvframe, struct __queue *pfree_recv_queue) +{ + struct adapter *padapter = precvframe->u.hdr.adapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + + rtw_os_free_recvframe(precvframe); + + + spin_lock_bh(&pfree_recv_queue->lock); + + list_del_init(&(precvframe->u.hdr.list)); + + precvframe->u.hdr.len = 0; + + list_add_tail(&(precvframe->u.hdr.list), get_list_head(pfree_recv_queue)); + + if (padapter != NULL) { + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + } + spin_unlock_bh(&pfree_recv_queue->lock); + return _SUCCESS; +} + + + + +sint _rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue) +{ + + struct adapter *padapter = precvframe->u.hdr.adapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + + /* INIT_LIST_HEAD(&(precvframe->u.hdr.list)); */ + list_del_init(&(precvframe->u.hdr.list)); + + + list_add_tail(&(precvframe->u.hdr.list), get_list_head(queue)); + + if (padapter != NULL) + if (queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + + return _SUCCESS; +} + +sint rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue) +{ + sint ret; + + /* _spinlock(&pfree_recv_queue->lock); */ + spin_lock_bh(&queue->lock); + ret = _rtw_enqueue_recvframe(precvframe, queue); + /* spin_unlock(&pfree_recv_queue->lock); */ + spin_unlock_bh(&queue->lock); + + return ret; +} + +/* +sint rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue) +{ + return rtw_free_recvframe(precvframe, queue); +} +*/ + + + + +/* +caller : defrag ; recvframe_chk_defrag in recv_thread (passive) +pframequeue: defrag_queue : will be accessed in recv_thread (passive) + +using spinlock to protect + +*/ + +void rtw_free_recvframe_queue(struct __queue *pframequeue, struct __queue *pfree_recv_queue) +{ + union recv_frame *precvframe; + struct list_head *plist, *phead; + + spin_lock(&pframequeue->lock); + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (phead != plist) { + precvframe = LIST_CONTAINOR(plist, union recv_frame, u); + + plist = get_next(plist); + + rtw_free_recvframe(precvframe, pfree_recv_queue); + } + + spin_unlock(&pframequeue->lock); +} + +u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter) +{ + u32 cnt = 0; + union recv_frame *pending_frame; + while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) { + rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue); + cnt++; + } + + if (cnt) + DBG_871X(FUNC_ADPT_FMT" dequeue %d\n", FUNC_ADPT_ARG(adapter), cnt); + + return cnt; +} + + +sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue) +{ + spin_lock_bh(&queue->lock); + + list_del_init(&precvbuf->list); + list_add(&precvbuf->list, get_list_head(queue)); + + spin_unlock_bh(&queue->lock); + + return _SUCCESS; +} + +sint rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue) +{ + spin_lock_bh(&queue->lock); + + list_del_init(&precvbuf->list); + + list_add_tail(&precvbuf->list, get_list_head(queue)); + spin_unlock_bh(&queue->lock); + return _SUCCESS; + +} + +struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue) +{ + struct recv_buf *precvbuf; + struct list_head *plist, *phead; + + spin_lock_bh(&queue->lock); + + if (list_empty(&queue->queue)) + precvbuf = NULL; + else{ + phead = get_list_head(queue); + + plist = get_next(phead); + + precvbuf = LIST_CONTAINOR(plist, struct recv_buf, list); + + list_del_init(&precvbuf->list); + + } + + spin_unlock_bh(&queue->lock); + + return precvbuf; + +} + +sint recvframe_chkmic(struct adapter *adapter, union recv_frame *precvframe); +sint recvframe_chkmic(struct adapter *adapter, union recv_frame *precvframe) +{ + + sint 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->u.hdr.attrib; + struct security_priv *psecuritypriv = &adapter->securitypriv; + + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + stainfo = rtw_get_stainfo(&adapter->stapriv, &prxattrib->ta[0]); + + if (prxattrib->encrypt == _TKIP_) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:prxattrib->encrypt == _TKIP_\n")); + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n", + prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2], prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5])); + + /* calculate mic code */ + if (stainfo != NULL) { + if (IS_MCAST(prxattrib->ra)) { + /* mickey =&psecuritypriv->dot118021XGrprxmickey.skey[0]; */ + /* iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen; */ + /* rxdata_key_idx =(((iv[3])>>6)&0x3) ; */ + mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0]; + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic: bcmc key\n")); + /* DBG_871X("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d), pmlmeinfo->key_index(%d) , recv key_id(%d)\n", */ + /* psecuritypriv->dot118021XGrpKeyid, pmlmeinfo->key_index, rxdata_key_idx); */ + + if (psecuritypriv->binstallGrpkey == false) { + res = _FAIL; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n")); + DBG_871X("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"); + goto exit; + } + } else { + mickey = &stainfo->dot11tkiprxmickey.skey[0]; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic: unicast key\n")); + } + + datalen = precvframe->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len-prxattrib->icv_len-8;/* icv_len included the mic code */ + pframe = precvframe->u.hdr.rx_data; + payload = pframe+prxattrib->hdrlen+prxattrib->iv_len; + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n prxattrib->iv_len =%d prxattrib->icv_len =%d\n", prxattrib->iv_len, prxattrib->icv_len)); + + + rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0], (unsigned char)prxattrib->priority); /* care the length of the data */ + + pframemic = payload+datalen; + + bmic_err = false; + + for (i = 0; i < 8; i++) { + if (miccode[i] != *(pframemic+i)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic:miccode[%d](%02x) != *(pframemic+%d)(%02x) ", i, miccode[i], i, *(pframemic+i))); + bmic_err = true; + } + } + + + if (bmic_err == true) { + + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-8)-*(pframemic-1) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n", + *(pframemic-8), *(pframemic-7), *(pframemic-6), *(pframemic-5), *(pframemic-4), *(pframemic-3), *(pframemic-2), *(pframemic-1))); + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-16)-*(pframemic-9) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n", + *(pframemic-16), *(pframemic-15), *(pframemic-14), *(pframemic-13), *(pframemic-12), *(pframemic-11), *(pframemic-10), *(pframemic-9))); + + { + uint i; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet (len =%d) ======\n", precvframe->u.hdr.len)); + for (i = 0; i < precvframe->u.hdr.len; i = i+8) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x", + *(precvframe->u.hdr.rx_data+i), *(precvframe->u.hdr.rx_data+i+1), + *(precvframe->u.hdr.rx_data+i+2), *(precvframe->u.hdr.rx_data+i+3), + *(precvframe->u.hdr.rx_data+i+4), *(precvframe->u.hdr.rx_data+i+5), + *(precvframe->u.hdr.rx_data+i+6), *(precvframe->u.hdr.rx_data+i+7))); + } + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet end [len =%d]======\n", precvframe->u.hdr.len)); + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n hrdlen =%d,\n", prxattrib->hdrlen)); + } + + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ra = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x psecuritypriv->binstallGrpkey =%d ", + prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2], + prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5], psecuritypriv->binstallGrpkey)); + + /* double check key_index for some timing issue , */ + /* cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */ + if ((IS_MCAST(prxattrib->ra) == true) && (prxattrib->key_index != pmlmeinfo->key_index)) + brpt_micerror = false; + + if ((prxattrib->bdecrypted == true) && (brpt_micerror == true)) { + rtw_handle_tkip_mic_err(adapter, (u8)IS_MCAST(prxattrib->ra)); + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted)); + DBG_871X(" 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_871X(" mic error :prxattrib->bdecrypted =%d\n", prxattrib->bdecrypted); + } + + res = _FAIL; + + } else { + /* mic checked ok */ + if ((psecuritypriv->bcheck_grpkey == false) && (IS_MCAST(prxattrib->ra) == true)) { + psecuritypriv->bcheck_grpkey = true; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("psecuritypriv->bcheck_grpkey =true")); + } + } + + } else + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic: rtw_get_stainfo == NULL!!!\n")); + + recvframe_pull_tail(precvframe, 8); + + } + +exit: + return res; + +} + +/* decrypt and set the ivlen, icvlen of the recv_frame */ +union recv_frame *decryptor(struct adapter *padapter, union recv_frame *precv_frame); +union recv_frame *decryptor(struct adapter *padapter, union recv_frame *precv_frame) +{ + + struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + union recv_frame *return_packet = precv_frame; + u32 res = _SUCCESS; + + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt); + + 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->u.hdr.rx_data+prxattrib->hdrlen; + prxattrib->key_index = (((iv[3])>>6)&0x3); + + if (prxattrib->key_index > WEP_KEYS) { + DBG_871X("prxattrib->key_index(%d) > WEP_KEYS\n", prxattrib->key_index); + + switch (prxattrib->encrypt) { + case _WEP40_: + case _WEP104_: + prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex; + break; + case _TKIP_: + case _AES_: + default: + prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid; + break; + } + } + } + + if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt == true))) { + psecuritypriv->hw_decrypted = false; + + #ifdef DBG_RX_DECRYPTOR + DBG_871X("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __func__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); + #endif + + switch (prxattrib->encrypt) { + case _WEP40_: + case _WEP104_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wep); + rtw_wep_decrypt(padapter, (u8 *)precv_frame); + break; + case _TKIP_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_tkip); + res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame); + break; + case _AES_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_aes); + res = rtw_aes_decrypt(padapter, (u8 *)precv_frame); + break; + default: + break; + } + } else if (prxattrib->bdecrypted == 1 + && prxattrib->encrypt > 0 + && (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_) + ) { + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_hw); + + psecuritypriv->hw_decrypted = true; + #ifdef DBG_RX_DECRYPTOR + DBG_871X("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __func__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); + + #endif + } else { + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_unknown); + #ifdef DBG_RX_DECRYPTOR + DBG_871X("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __func__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); + #endif + } + + if (res == _FAIL) { + rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue); + return_packet = NULL; + } else + prxattrib->bdecrypted = true; + + return return_packet; +} + +/* set the security information in the recv_frame */ +union recv_frame *portctrl(struct adapter *adapter, union recv_frame *precv_frame); +union recv_frame *portctrl(struct adapter *adapter, union recv_frame *precv_frame) +{ + u8 *psta_addr = NULL; + u8 *ptr; + uint auth_alg; + struct recv_frame_hdr *pfhdr; + struct sta_info *psta; + struct sta_priv *pstapriv; + union 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; + + auth_alg = adapter->securitypriv.dot11AuthAlgrthm; + + ptr = get_recvframe_data(precv_frame); + pfhdr = &precv_frame->u.hdr; + pattrib = &pfhdr->attrib; + psta_addr = pattrib->ta; + + prtnframe = NULL; + + psta = rtw_get_stainfo(pstapriv, psta_addr); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm =%d\n", adapter->securitypriv.dot11AuthAlgrthm)); + + if (auth_alg == 2) { + if ((psta != NULL) && (psta->ieee8021x_blocked)) { + __be16 be_tmp; + + /* blocked */ + /* only accept EAPOL frame */ + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 1\n")); + + prtnframe = precv_frame; + + /* get ether_type */ + ptr = ptr+pfhdr->attrib.hdrlen+pfhdr->attrib.iv_len+LLC_HEADER_SIZE; + memcpy(&be_tmp, ptr, 2); + ether_type = ntohs(be_tmp); + + if (ether_type == eapol_type) + prtnframe = precv_frame; + else { + /* free this frame */ + rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue); + prtnframe = NULL; + } + } else{ + /* allowed */ + /* check decryption status, and decrypt the frame if needed */ + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 0\n")); + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:precv_frame->hdr.attrib.privacy =%x\n", precv_frame->u.hdr.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; +} + +sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache); +sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache) +{ + sint tid = precv_frame->u.hdr.attrib.priority; + + u16 seq_ctrl = ((precv_frame->u.hdr.attrib.seq_num&0xffff) << 4) | + (precv_frame->u.hdr.attrib.frag_num & 0xf); + + if (tid > 15) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, (tid>15)! seq_ctrl = 0x%x, tid = 0x%x\n", seq_ctrl, tid)); + + return _FAIL; + } + + if (1) { /* if (bretry) */ + if (seq_ctrl == prxcache->tid_rxseq[tid]) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, seq_ctrl = 0x%x, tid = 0x%x, tid_rxseq = 0x%x\n", seq_ctrl, tid, prxcache->tid_rxseq[tid])); + + return _FAIL; + } + } + + prxcache->tid_rxseq[tid] = seq_ctrl; + + return _SUCCESS; + +} + +void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame); +void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame) +{ + unsigned char pwrbit; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + + psta = rtw_get_stainfo(pstapriv, pattrib->src); + + pwrbit = GetPwrMgt(ptr); + + if (psta) { + if (pwrbit) { + if (!(psta->state & WIFI_SLEEP_STATE)) { + /* psta->state |= WIFI_SLEEP_STATE; */ + /* pstapriv->sta_dz_bitmap |= BIT(psta->aid); */ + + stop_sta_xmit(padapter, psta); + + /* DBG_871X("to sleep, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */ + } + } else{ + if (psta->state & WIFI_SLEEP_STATE) { + /* psta->state ^= WIFI_SLEEP_STATE; */ + /* pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); */ + + wakeup_sta_to_xmit(padapter, psta); + + /* DBG_871X("to wakeup, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */ + } + } + + } +} + +void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame); +void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + + psta = rtw_get_stainfo(pstapriv, pattrib->src); + + if (!psta) + return; + + if (!psta->qos_option) + return; + + if (!(psta->qos_info&0xf)) + return; + + if (psta->state&WIFI_SLEEP_STATE) { + u8 wmmps_ac = 0; + + switch (pattrib->priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk&BIT(1); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi&BIT(1); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo&BIT(1); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be&BIT(1); + break; + } + + if (wmmps_ac) { + if (psta->sleepq_ac_len > 0) + /* process received triggered frame */ + xmit_delivery_enabled_frames(padapter, psta); + else + /* issue one qos null frame with More data bit = 0 and the EOSP bit set (= 1) */ + issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0); + } + } +} + +void count_rx_stats(struct adapter *padapter, union recv_frame *prframe, struct sta_info *sta); +void count_rx_stats(struct adapter *padapter, union 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->u.hdr.attrib; + struct recv_priv *precvpriv = &padapter->recvpriv; + + sz = get_recvframe_len(prframe); + precvpriv->rx_bytes += sz; + + padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++; + + if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst))) { + padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++; + } + + if (sta) + psta = sta; + else + psta = prframe->u.hdr.psta; + + if (psta) { + pstats = &psta->sta_stats; + + pstats->rx_data_pkts++; + pstats->rx_bytes += sz; + } + + traffic_check_for_leave_lps(padapter, false, 0); +} + +sint sta2sta_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta +); +sint sta2sta_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta +) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + sint ret = _SUCCESS; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.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; + sint bmcast = IS_MCAST(pattrib->dst); + + /* DBG_871X("[%s] %d, seqnum:%d\n", __func__, __LINE__, pattrib->seq_num); */ + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) { + + /* filter packets that SA is myself or multicast or broadcast */ + if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n")); + ret = _FAIL; + goto exit; + } + + if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) { + ret = _FAIL; + goto exit; + } + + if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) { + ret = _FAIL; + goto exit; + } + + sta_addr = pattrib->src; + + } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) { + /* For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */ + if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("bssid != TA under STATION_MODE; drop pkt\n")); + ret = _FAIL; + goto exit; + } + + sta_addr = pattrib->bssid; + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + if (bmcast) { + /* For AP mode, if DA == MCAST, then BSSID should be also MCAST */ + if (!IS_MCAST(pattrib->bssid)) { + ret = _FAIL; + goto exit; + } + } else{ /* not mc-frame */ + /* For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */ + if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) { + ret = _FAIL; + goto exit; + } + + sta_addr = pattrib->src; + } + + } else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) { + memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN); + memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + sta_addr = mybssid; + } else + ret = _FAIL; + + + + if (bmcast) + *psta = rtw_get_bcmc_stainfo(adapter); + else + *psta = rtw_get_stainfo(pstapriv, sta_addr); /* get ap_info */ + + if (*psta == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under sta2sta_data_frame ; drop pkt\n")); + ret = _FAIL; + goto exit; + } + +exit: + return ret; +} + +sint ap2sta_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta); +sint ap2sta_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + sint ret = _SUCCESS; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u8 *mybssid = get_bssid(pmlmepriv); + u8 *myhwaddr = myid(&adapter->eeprompriv); + sint bmcast = IS_MCAST(pattrib->dst); + + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) + && (check_fwstate(pmlmepriv, _FW_LINKED) == true + || check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) + ) { + + /* filter packets that SA is myself or multicast or broadcast */ + if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s SA ="MAC_FMT", myhwaddr ="MAC_FMT"\n", + __func__, MAC_ARG(pattrib->src), MAC_ARG(myhwaddr)); + #endif + ret = _FAIL; + goto exit; + } + + /* da should be for me */ + if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + (" ap2sta_data_frame: compare DA fail; DA ="MAC_FMT"\n", MAC_ARG(pattrib->dst))); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s DA ="MAC_FMT"\n", __func__, MAC_ARG(pattrib->dst)); + #endif + ret = _FAIL; + goto exit; + } + + + /* check BSSID */ + if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, + (" ap2sta_data_frame: compare BSSID fail ; BSSID ="MAC_FMT"\n", MAC_ARG(pattrib->bssid))); + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("mybssid ="MAC_FMT"\n", MAC_ARG(mybssid))); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s BSSID ="MAC_FMT", mybssid ="MAC_FMT"\n", + __func__, MAC_ARG(pattrib->bssid), MAC_ARG(mybssid)); + DBG_871X("this adapter = %d, buddy adapter = %d\n", adapter->adapter_type, adapter->pbuddystruct adapter->adapter_type); + #endif + + if (!bmcast) { + DBG_871X("issue_deauth to the nonassociated ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid)); + issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + + ret = _FAIL; + goto exit; + } + + if (bmcast) + *psta = rtw_get_bcmc_stainfo(adapter); + else + *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get ap_info */ + + if (*psta == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ap2sta: can't get psta under STATION_MODE ; drop pkt\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under STATION_MODE ; drop pkt\n", __func__); + #endif + ret = _FAIL; + goto exit; + } + + if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) { + } + + if (GetFrameSubType(ptr) & BIT(6)) { + /* No data, will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, precv_frame, *psta); + ret = RTW_RX_HANDLED; + goto exit; + } + + } else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && + (check_fwstate(pmlmepriv, _FW_LINKED) == true)) { + memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN); + memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + /* */ + memcpy(pattrib->bssid, mybssid, ETH_ALEN); + + + *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */ + if (*psta == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under MP_MODE ; drop pkt\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __func__); + #endif + ret = _FAIL; + goto exit; + } + + + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + /* Special case */ + ret = RTW_RX_HANDLED; + goto exit; + } else{ + if (!memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) { + *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */ + if (*psta == NULL) { + + /* for AP multicast issue , modify by yiwei */ + static unsigned long send_issue_deauth_time = 0; + + /* DBG_871X("After send deauth , %u ms has elapsed.\n", jiffies_to_msecs(jiffies - send_issue_deauth_time)); */ + + if (jiffies_to_msecs(jiffies - send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) { + send_issue_deauth_time = jiffies; + + DBG_871X("issue_deauth to the ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid)); + + issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + } + } + + ret = _FAIL; + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s fw_state:0x%x\n", __func__, get_fwstate(pmlmepriv)); + #endif + } + +exit: + return ret; +} + +sint sta2ap_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta); +sint sta2ap_data_frame( + struct adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + unsigned char *mybssid = get_bssid(pmlmepriv); + sint ret = _SUCCESS; + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) { + /* For AP mode, RA =BSSID, TX =STA(SRC_ADDR), A3 =DST_ADDR */ + if (memcmp(pattrib->bssid, mybssid, ETH_ALEN)) { + ret = _FAIL; + goto exit; + } + + *psta = rtw_get_stainfo(pstapriv, pattrib->src); + if (*psta == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under AP_MODE; drop pkt\n")); + DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src)); + + issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + + ret = RTW_RX_HANDLED; + goto exit; + } + + process_pwrbit_data(adapter, precv_frame); + + if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) { + process_wmmps_data(adapter, precv_frame); + } + + if (GetFrameSubType(ptr) & BIT(6)) { + /* No data, will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, precv_frame, *psta); + ret = RTW_RX_HANDLED; + goto exit; + } + } else { + u8 *myhwaddr = myid(&adapter->eeprompriv); + if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) { + ret = RTW_RX_HANDLED; + goto exit; + } + DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src)); + issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + ret = RTW_RX_HANDLED; + goto exit; + } + +exit: + return ret; +} + +sint validate_recv_ctrl_frame(struct adapter *padapter, union recv_frame *precv_frame); +sint validate_recv_ctrl_frame(struct adapter *padapter, union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_info *psta = NULL; + /* uint len = precv_frame->u.hdr.len; */ + + /* DBG_871X("+validate_recv_ctrl_frame\n"); */ + + if (GetFrameType(pframe) != WIFI_CTRL_TYPE) + return _FAIL; + + /* receive the frames that ra(a1) is my address */ + if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN)) + return _FAIL; + + psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe)); + if (psta == NULL) + return _FAIL; + + /* for rx pkt statistics */ + psta->sta_stats.rx_ctrl_pkts++; + + /* only handle ps-poll */ + if (GetFrameSubType(pframe) == WIFI_PSPOLL) { + u16 aid; + u8 wmmps_ac = 0; + + aid = GetAid(pframe); + if (psta->aid != aid) + return _FAIL; + + 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_871X("%s alive check-rx ps-poll\n", __func__); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + + if ((psta->state&WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap&BIT(psta->aid))) { + struct list_head *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + /* spin_lock_bh(&psta->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + if (xmitframe_phead != xmitframe_plist) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + 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_871X("handling ps-poll, q_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */ + + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + if (psta->sleepq_len == 0) { + pstapriv->tim_bitmap &= ~BIT(psta->aid); + + /* DBG_871X("after handling ps-poll, tim =%x\n", pstapriv->tim_bitmap); */ + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, true); + } + + /* 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_871X("no buffered packets to xmit\n"); */ + if (pstapriv->tim_bitmap&BIT(psta->aid)) { + if (psta->sleepq_len == 0) { + DBG_871X("no buffered packets to xmit\n"); + + /* issue nulldata with More data bit = 0 to indicate we have no buffered packets */ + issue_nulldata_in_interrupt(padapter, psta->hwaddr); + } else{ + DBG_871X("error!psta->sleepq_len =%d\n", psta->sleepq_len); + psta->sleepq_len = 0; + } + + pstapriv->tim_bitmap &= ~BIT(psta->aid); + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, true); + } + } + } + } + + return _FAIL; + +} + +union recv_frame *recvframe_chk_defrag(struct adapter *padapter, union recv_frame *precv_frame); +sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame); +sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame) +{ + /* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */ + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+validate_recv_mgnt_frame\n")); + + precv_frame = recvframe_chk_defrag(padapter, precv_frame); + if (precv_frame == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("%s: fragment packet\n", __func__)); + return _SUCCESS; + } + + { + /* for rx pkt statistics */ + struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(precv_frame->u.hdr.rx_data)); + if (psta) { + psta->sta_stats.rx_mgnt_pkts++; + if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_BEACON) + psta->sta_stats.rx_beacon_pkts++; + else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ) + psta->sta_stats.rx_probereq_pkts++; + else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) { + if (!memcmp(padapter->eeprompriv.mac_addr, GetAddr1Ptr(precv_frame->u.hdr.rx_data), ETH_ALEN)) + psta->sta_stats.rx_probersp_pkts++; + else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)) + || is_multicast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data))) + psta->sta_stats.rx_probersp_bm_pkts++; + else + psta->sta_stats.rx_probersp_uo_pkts++; + } + } + } + + mgt_dispatcher(padapter, precv_frame); + + return _SUCCESS; + +} + +sint validate_recv_data_frame(struct adapter *adapter, union recv_frame *precv_frame); +sint validate_recv_data_frame(struct adapter *adapter, union recv_frame *precv_frame) +{ + u8 bretry; + u8 *psa, *pda, *pbssid; + struct sta_info *psta = NULL; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct security_priv *psecuritypriv = &adapter->securitypriv; + sint ret = _SUCCESS; + + bretry = GetRetry(ptr); + pda = get_da(ptr); + psa = get_sa(ptr); + pbssid = get_hdr_bssid(ptr); + + if (pbssid == NULL) { + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s pbssid == NULL\n", __func__); + #endif + ret = _FAIL; + goto exit; + } + + memcpy(pattrib->dst, pda, ETH_ALEN); + memcpy(pattrib->src, psa, ETH_ALEN); + + memcpy(pattrib->bssid, pbssid, ETH_ALEN); + + switch (pattrib->to_fr_ds) { + case 0: + memcpy(pattrib->ra, pda, ETH_ALEN); + memcpy(pattrib->ta, psa, ETH_ALEN); + ret = sta2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 1: + memcpy(pattrib->ra, pda, ETH_ALEN); + memcpy(pattrib->ta, pbssid, ETH_ALEN); + ret = ap2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 2: + memcpy(pattrib->ra, pbssid, ETH_ALEN); + memcpy(pattrib->ta, psa, ETH_ALEN); + ret = sta2ap_data_frame(adapter, precv_frame, &psta); + break; + + case 3: + memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN); + ret = _FAIL; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n")); + break; + + default: + ret = _FAIL; + break; + + } + + if (ret == _FAIL) { + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s case:%d, res:%d\n", __func__, pattrib->to_fr_ds, ret); + #endif + goto exit; + } else if (ret == RTW_RX_HANDLED) { + goto exit; + } + + + if (psta == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" after to_fr_ds_chk; psta == NULL\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s psta == NULL\n", __func__); + #endif + ret = _FAIL; + goto exit; + } + + /* psta->rssi = prxcmd->rssi; */ + /* psta->signal_quality = prxcmd->sq; */ + precv_frame->u.hdr.psta = psta; + + + pattrib->amsdu = 0; + pattrib->ack_policy = 0; + /* parsing QC field */ + if (pattrib->qos == 1) { + pattrib->priority = GetPriority((ptr + 24)); + pattrib->ack_policy = GetAckpolicy((ptr + 24)); + pattrib->amsdu = GetAMsdu((ptr + 24)); + pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26; + + if (pattrib->priority != 0 && pattrib->priority != 3) + adapter->recvpriv.bIsAnyNonBEPkts = true; + + } else{ + pattrib->priority = 0; + pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24; + } + + + if (pattrib->order)/* HT-CTRL 11n */ + pattrib->hdrlen += 4; + + precv_frame->u.hdr.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")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s recv_decache return _FAIL\n", __func__); + #endif + ret = _FAIL; + goto exit; + } + + if (pattrib->privacy) { + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("validate_recv_data_frame:pattrib->privacy =%x\n", pattrib->privacy)); + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0], IS_MCAST(pattrib->ra))); + + GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra)); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n pattrib->encrypt =%d\n", pattrib->encrypt)); + + SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt); + } else{ + pattrib->encrypt = 0; + pattrib->iv_len = pattrib->icv_len = 0; + } + +exit: + return ret; +} + +static sint validate_80211w_mgmt(struct adapter *adapter, union recv_frame *precv_frame) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + u8 *ptr = precv_frame->u.hdr.rx_data; + u8 type; + u8 subtype; + + type = GetFrameType(ptr); + subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */ + + /* only support station mode */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && check_fwstate(pmlmepriv, _FW_LINKED) + && adapter->securitypriv.binstallBIPkey == true) { + /* unicast management frame decrypt */ + if (pattrib->privacy && !(IS_MCAST(GetAddr1Ptr(ptr))) && + (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC || subtype == WIFI_ACTION)) { + u8 *ppp, *mgmt_DATA; + u32 data_len = 0; + ppp = GetAddr2Ptr(ptr); + + pattrib->bdecrypted = 0; + pattrib->encrypt = _AES_; + pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr); + /* set iv and icv length */ + SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt); + memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN); + /* actual management data frame body */ + data_len = pattrib->pkt_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + mgmt_DATA = rtw_zmalloc(data_len); + if (mgmt_DATA == NULL) { + DBG_871X("%s mgmt allocate fail !!!!!!!!!\n", __func__); + goto validate_80211w_fail; + } + precv_frame = decryptor(adapter, precv_frame); + /* save actual management data frame body */ + memcpy(mgmt_DATA, ptr+pattrib->hdrlen+pattrib->iv_len, data_len); + /* overwrite the iv field */ + memcpy(ptr+pattrib->hdrlen, mgmt_DATA, data_len); + /* remove the iv and icv length */ + pattrib->pkt_len = pattrib->pkt_len - pattrib->iv_len - pattrib->icv_len; + kfree(mgmt_DATA); + if (!precv_frame) { + DBG_871X("%s mgmt descrypt fail !!!!!!!!!\n", __func__); + goto validate_80211w_fail; + } + } else if (IS_MCAST(GetAddr1Ptr(ptr)) && + (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC)) { + sint BIP_ret = _SUCCESS; + /* verify BIP MME IE of broadcast/multicast de-auth/disassoc packet */ + BIP_ret = rtw_BIP_verify(adapter, (u8 *)precv_frame); + if (BIP_ret == _FAIL) { + /* DBG_871X("802.11w BIP verify fail\n"); */ + goto validate_80211w_fail; + } else if (BIP_ret == RTW_RX_HANDLED) { + /* DBG_871X("802.11w recv none protected packet\n"); */ + /* issue sa query request */ + issue_action_SA_Query(adapter, NULL, 0, 0); + goto validate_80211w_fail; + } + } else { /* 802.11w protect */ + if (subtype == WIFI_ACTION) { + /* according 802.11-2012 standard, these five types are not robust types */ + if (ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_PUBLIC && + ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_HT && + ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_UNPROTECTED_WNM && + ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_SELF_PROTECTED && + ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_P2P) { + DBG_871X("action frame category =%d should robust\n", ptr[WLAN_HDR_A3_LEN]); + goto validate_80211w_fail; + } + } else if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) { + DBG_871X("802.11w recv none protected packet\n"); + /* issue sa query request */ + issue_action_SA_Query(adapter, NULL, 0, 0); + goto validate_80211w_fail; + } + } + } + return _SUCCESS; + +validate_80211w_fail: + return _FAIL; + +} + +static inline void dump_rx_packet(u8 *ptr) +{ + int i; + + DBG_871X("#############################\n"); + for (i = 0; i < 64; i = i+8) + DBG_871X("%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_871X("#############################\n"); +} + +sint validate_recv_frame(struct adapter *adapter, union recv_frame *precv_frame); +sint validate_recv_frame(struct adapter *adapter, union 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; + sint retval = _SUCCESS; + u8 bDumpRxPkt; + + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + + u8 *ptr = precv_frame->u.hdr.rx_data; + u8 ver = (unsigned char) (*ptr)&0x3; + + /* add version chk */ + if (ver != 0) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! (ver!= 0)\n")); + retval = _FAIL; + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ver_err); + goto exit; + } + + type = GetFrameType(ptr); + subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */ + + pattrib->to_fr_ds = get_tofr_ds(ptr); + + pattrib->frag_num = GetFragNum(ptr); + pattrib->seq_num = GetSequence(ptr); + + pattrib->pw_save = GetPwrMgt(ptr); + pattrib->mfrag = GetMFrag(ptr); + pattrib->mdata = GetMData(ptr); + pattrib->privacy = GetPrivacy(ptr); + pattrib->order = GetOrder(ptr); + rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt)); + if (bDumpRxPkt == 1) /* dump all rx packets */ + dump_rx_packet(ptr); + else if ((bDumpRxPkt == 2) && (type == WIFI_MGT_TYPE)) + dump_rx_packet(ptr); + else if ((bDumpRxPkt == 3) && (type == WIFI_DATA_TYPE)) + dump_rx_packet(ptr); + + switch (type) { + case WIFI_MGT_TYPE: /* mgnt */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt); + if (validate_80211w_mgmt(adapter, precv_frame) == _FAIL) { + retval = _FAIL; + DBG_COUNTER(padapter->rx_logs.core_rx_pre_mgmt_err_80211w); + break; + } + + 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")); + DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt_err); + } + retval = _FAIL; /* only data frame return _SUCCESS */ + break; + case WIFI_CTRL_TYPE: /* ctrl */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl); + retval = validate_recv_ctrl_frame(adapter, precv_frame); + if (retval == _FAIL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_ctrl_frame fail\n")); + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl_err); + } + retval = _FAIL; /* only data frame return _SUCCESS */ + break; + case WIFI_DATA_TYPE: /* data */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data); + + pattrib->qos = (subtype & BIT(7)) ? 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++; + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_err); + } else if (retval == _SUCCESS) { +#ifdef DBG_RX_DUMP_EAP + u8 bDumpRxPkt; + u16 eth_type; + + /* dump eapol */ + rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt)); + /* get ether_type */ + memcpy(ð_type, ptr + pattrib->hdrlen + pattrib->iv_len + LLC_HEADER_SIZE, 2); + eth_type = ntohs((unsigned short) eth_type); + if ((bDumpRxPkt == 4) && (eth_type == 0x888e)) + dump_rx_packet(ptr); +#endif + } else + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_handled); + break; + default: + DBG_COUNTER(adapter->rx_logs.core_rx_pre_unknown); + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! type = 0x%x\n", type)); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME validate_recv_data_frame fail! type = 0x%x\n", type); + #endif + retval = _FAIL; + break; + } + +exit: + return retval; +} + + +/* remove the wlanhdr and add the eth_hdr */ +sint wlanhdr_to_ethhdr(union recv_frame *precvframe); +sint wlanhdr_to_ethhdr(union recv_frame *precvframe) +{ + sint rmv_len; + u16 eth_type, len; + u8 bsnaphdr; + u8 *psnap_type; + struct ieee80211_snap_hdr *psnap; + __be16 be_tmp; + sint ret = _SUCCESS; + struct adapter *adapter = precvframe->u.hdr.adapter; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u8 *ptr = get_recvframe_data(precvframe) ; /* point to frame_ctrl field */ + struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib; + + if (pattrib->encrypt) { + recvframe_pull_tail(precvframe, pattrib->icv_len); + } + + psnap = (struct ieee80211_snap_hdr *)(ptr+pattrib->hdrlen + pattrib->iv_len); + psnap_type = ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE; + /* convert hdr + possible LLC headers into Ethernet header */ + /* eth_type = (psnap_type[0] << 8) | psnap_type[1]; */ + if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) && + (memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2)) && + (memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2))) || + /* eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || */ + !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) { + /* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */ + bsnaphdr = true; + } else + /* Leave Ethernet header part of hdr and full payload */ + bsnaphdr = false; + + rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr?SNAP_SIZE:0); + len = precvframe->u.hdr.len - rmv_len; + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ===pattrib->hdrlen: %x, pattrib->iv_len:%x ===\n\n", pattrib->hdrlen, pattrib->iv_len)); + + memcpy(&be_tmp, ptr+rmv_len, 2); + eth_type = ntohs(be_tmp); /* pattrib->ether_type */ + pattrib->eth_type = eth_type; + +#ifdef CONFIG_AUTO_AP_MODE + if (0x8899 == pattrib->eth_type) { + struct sta_info *psta = precvframe->u.hdr.psta; + + DBG_871X("wlan rx: got eth_type = 0x%x\n", pattrib->eth_type); + + if (psta && psta->isrc && psta->pid > 0) { + u16 rx_pid; + + rx_pid = *(u16 *)(ptr+rmv_len+2); + + DBG_871X("wlan rx(pid = 0x%x): sta("MAC_FMT") pid = 0x%x\n", + rx_pid, MAC_ARG(psta->hwaddr), psta->pid); + + if (rx_pid == psta->pid) { + int i; + u16 len = *(u16 *)(ptr+rmv_len+4); + /* u16 ctrl_type = *(u16*)(ptr+rmv_len+6); */ + + /* DBG_871X("RC: len = 0x%x, ctrl_type = 0x%x\n", len, ctrl_type); */ + DBG_871X("RC: len = 0x%x\n", len); + + for (i = 0; i < len ; i++) + DBG_871X("0x%x\n", *(ptr+rmv_len+6+i)); + /* DBG_871X("0x%x\n", *(ptr+rmv_len+8+i)); */ + + DBG_871X("RC-end\n"); + } + } + } +#endif /* CONFIG_AUTO_AP_MODE */ + + if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) { + ptr += rmv_len; + *ptr = 0x87; + *(ptr+1) = 0x12; + + eth_type = 0x8712; + /* append rx status for mp test packets */ + ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24); + memcpy(ptr, get_rxmem(precvframe), 24); + ptr += 24; + } else + ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr) + (bsnaphdr?2:0))); + + memcpy(ptr, pattrib->dst, ETH_ALEN); + memcpy(ptr+ETH_ALEN, pattrib->src, ETH_ALEN); + + if (!bsnaphdr) { + be_tmp = htons(len); + memcpy(ptr+12, &be_tmp, 2); + } + + return ret; +} + +/* perform defrag */ +static union recv_frame *recvframe_defrag(struct adapter *adapter, + struct __queue *defrag_q) +{ + struct list_head *plist, *phead; + u8 *data, wlanhdr_offset; + u8 curfragnum; + struct recv_frame_hdr *pfhdr, *pnfhdr; + union recv_frame *prframe, *pnextrframe; + struct __queue *pfree_recv_queue; + + curfragnum = 0; + pfree_recv_queue = &adapter->recvpriv.free_recv_queue; + + phead = get_list_head(defrag_q); + plist = get_next(phead); + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + pfhdr = &prframe->u.hdr; + list_del_init(&(prframe->u.list)); + + if (curfragnum != pfhdr->attrib.frag_num) { + /* the first fragment number must be 0 */ + /* free the whole queue */ + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + + return NULL; + } + + curfragnum++; + + plist = get_list_head(defrag_q); + + plist = get_next(plist); + + data = get_recvframe_data(prframe); + + while (phead != plist) { + pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u); + pnfhdr = &pnextrframe->u.hdr; + + + /* check the fragment sequence (2nd ~n fragment frame) */ + + if (curfragnum != pnfhdr->attrib.frag_num) { + /* the fragment number must be increasing (after decache) */ + /* release the defrag_q & prframe */ + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + return NULL; + } + + curfragnum++; + + /* copy the 2nd~n fragment frame's payload to the first fragment */ + /* get the 2nd~last fragment frame's payload */ + + wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len; + + recvframe_pull(pnextrframe, wlanhdr_offset); + + /* append to first fragment frame's tail (if privacy frame, pull the ICV) */ + recvframe_pull_tail(prframe, pfhdr->attrib.icv_len); + + /* memcpy */ + memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len); + + recvframe_put(prframe, pnfhdr->len); + + pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len; + plist = get_next(plist); + + }; + + /* free the defrag_q queue and return the prframe */ + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Performance defrag!!!!!\n")); + + return prframe; +} + +/* check if need to defrag, if needed queue the frame to defrag_q */ +union recv_frame *recvframe_chk_defrag(struct adapter *padapter, union recv_frame *precv_frame) +{ + u8 ismfrag; + u8 fragnum; + u8 *psta_addr; + struct recv_frame_hdr *pfhdr; + struct sta_info *psta; + struct sta_priv *pstapriv; + struct list_head *phead; + union recv_frame *prtnframe = NULL; + struct __queue *pfree_recv_queue, *pdefrag_q; + + pstapriv = &padapter->stapriv; + + pfhdr = &precv_frame->u.hdr; + + pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + + /* need to define struct of wlan header frame ctrl */ + ismfrag = pfhdr->attrib.mfrag; + fragnum = pfhdr->attrib.frag_num; + + psta_addr = pfhdr->attrib.ta; + psta = rtw_get_stainfo(pstapriv, psta_addr); + if (psta == NULL) { + u8 type = GetFrameType(pfhdr->rx_data); + if (type != WIFI_DATA_TYPE) { + psta = rtw_get_bcmc_stainfo(padapter); + pdefrag_q = &psta->sta_recvpriv.defrag_q; + } else + pdefrag_q = NULL; + } else + pdefrag_q = &psta->sta_recvpriv.defrag_q; + + if ((ismfrag == 0) && (fragnum == 0)) + prtnframe = precv_frame;/* isn't a fragment frame */ + + if (ismfrag == 1) { + /* 0~(n-1) fragment frame */ + /* enqueue to defraf_g */ + if (pdefrag_q != NULL) { + if (fragnum == 0) + /* the first fragment */ + if (!list_empty(&pdefrag_q->queue)) + /* free current defrag_q */ + rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue); + + + /* Then enqueue the 0~(n-1) fragment into the defrag_q */ + + /* 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_recvframe(precv_frame, pfree_recv_queue); + prtnframe = NULL; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum)); + } + + } + + if ((ismfrag == 0) && (fragnum != 0)) { + /* the last fragment frame */ + /* enqueue the last fragment */ + if (pdefrag_q != NULL) { + /* 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_recvframe(precv_frame, pfree_recv_queue); + prtnframe = NULL; + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum)); + } + + } + + + if ((prtnframe != NULL) && (prtnframe->u.hdr.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_recvframe(prtnframe, pfree_recv_queue); + prtnframe = NULL; + } + } + return prtnframe; +} + +static int amsdu_to_msdu(struct adapter *padapter, union recv_frame *prframe) +{ + int a_len, padding_len; + u16 nSubframe_Length; + u8 nr_subframes, i; + u8 *pdata; + _pkt *sub_pkt, *subframes[MAX_SUBFRAME_COUNT]; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct __queue *pfree_recv_queue = &(precvpriv->free_recv_queue); + int ret = _SUCCESS; + + nr_subframes = 0; + + recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen); + + if (prframe->u.hdr.attrib.iv_len > 0) + recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len); + + a_len = prframe->u.hdr.len; + + pdata = prframe->u.hdr.rx_data; + + while (a_len > ETH_HLEN) { + + /* Offset 12 denote 2 mac address */ + nSubframe_Length = RTW_GET_BE16(pdata + 12); + + if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) { + DBG_871X("nRemain_Length is %d and nSubframe_Length is : %d\n", a_len, nSubframe_Length); + break; + } + + sub_pkt = rtw_os_alloc_msdu_pkt(prframe, nSubframe_Length, pdata); + if (sub_pkt == NULL) { + DBG_871X("%s(): allocate sub packet fail !!!\n", __func__); + break; + } + + /* move the data point to data content */ + pdata += ETH_HLEN; + a_len -= ETH_HLEN; + + subframes[nr_subframes++] = sub_pkt; + + if (nr_subframes >= MAX_SUBFRAME_COUNT) { + DBG_871X("ParseSubframe(): Too many Subframes! Packets dropped!\n"); + break; + } + + pdata += nSubframe_Length; + a_len -= nSubframe_Length; + if (a_len != 0) { + padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4-1)); + if (padding_len == 4) { + padding_len = 0; + } + + if (a_len < padding_len) { + DBG_871X("ParseSubframe(): a_len < padding_len !\n"); + break; + } + pdata += padding_len; + a_len -= padding_len; + } + } + + for (i = 0; i < nr_subframes; i++) { + sub_pkt = subframes[i]; + + /* Indicat the packets to upper layer */ + if (sub_pkt) { + rtw_os_recv_indicate_pkt(padapter, sub_pkt, &prframe->u.hdr.attrib); + } + } + + prframe->u.hdr.len = 0; + rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */ + + return ret; +} + +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) +{ + struct adapter *padapter = preorder_ctrl->padapter; + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + u8 wsize = preorder_ctrl->wsize_b; + u16 wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF;/* 4096; */ + + /* Rx Reorder initialize condition. */ + if (preorder_ctrl->indicate_seq == 0xFFFF) { + preorder_ctrl->indicate_seq = seq_num; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d init IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, seq_num); + #endif + + /* DbgPrint("check_indicate_seq, 1st->indicate_seq =%d\n", precvpriv->indicate_seq); */ + } + + /* DbgPrint("enter->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */ + + /* Drop out the packet which SeqNum is smaller than WinStart */ + if (SN_LESS(seq_num, preorder_ctrl->indicate_seq)) { + /* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */ + /* DbgPrint("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */ + + #ifdef DBG_RX_DROP_FRAME + DBG_871X("%s IndicateSeq: %d > NewSeq: %d\n", __func__, + preorder_ctrl->indicate_seq, seq_num); + #endif + + + 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; + + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d SN_EQUAL IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, seq_num); + #endif + } else if (SN_LESS(wend, seq_num)) { + /* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */ + /* DbgPrint("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, 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; + pdbgpriv->dbg_rx_ampdu_window_shift_cnt++; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d SN_LESS(wend, seq_num) IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, seq_num); + #endif + } + + /* DbgPrint("exit->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */ + + return true; +} + +int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe); +int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + struct list_head *phead, *plist; + union recv_frame *pnextrframe; + struct rx_pkt_attrib *pnextattrib; + + /* DbgPrint("+enqueue_reorder_recvframe()\n"); */ + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */ + /* spin_lock(&ppending_recvframe_queue->lock); */ + + + phead = get_list_head(ppending_recvframe_queue); + plist = get_next(phead); + + while (phead != plist) { + pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u); + pnextattrib = &pnextrframe->u.hdr.attrib; + + if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num)) + plist = get_next(plist); + 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, irql); */ + return false; + else + break; + + /* DbgPrint("enqueue_reorder_recvframe():while\n"); */ + + } + + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */ + /* spin_lock(&ppending_recvframe_queue->lock); */ + + list_del_init(&(prframe->u.hdr.list)); + + list_add_tail(&(prframe->u.hdr.list), plist); + + /* spin_unlock(&ppending_recvframe_queue->lock); */ + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */ + + + /* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */ + return true; + +} + +void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq); +void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq) +{ + if (current_seq < prev_seq) + pdbgpriv->dbg_rx_ampdu_loss_count += (4096 + current_seq - prev_seq); + else + pdbgpriv->dbg_rx_ampdu_loss_count += (current_seq - prev_seq); + +} +int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced); +int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced) +{ + struct list_head *phead, *plist; + union recv_frame *prframe; + struct rx_pkt_attrib *pattrib; + /* u8 index = 0; */ + int bPktInBuf = false; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_in_oder); + + /* DbgPrint("+recv_indicatepkts_in_order\n"); */ + + /* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */ + /* spin_lock(&ppending_recvframe_queue->lock); */ + + phead = get_list_head(ppending_recvframe_queue); + plist = get_next(phead); + + /* Handling some condition for forced indicate case. */ + if (bforced == true) { + pdbgpriv->dbg_rx_ampdu_forced_indicate_count++; + if (list_empty(phead)) { + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */ + /* spin_unlock(&ppending_recvframe_queue->lock); */ + return true; + } + + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + pattrib = &prframe->u.hdr.attrib; + + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + recv_indicatepkts_pkt_loss_cnt(pdbgpriv, preorder_ctrl->indicate_seq, pattrib->seq_num); + 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 = LIST_CONTAINOR(plist, union recv_frame, u); + pattrib = &prframe->u.hdr.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 = get_next(plist); + list_del_init(&(prframe->u.hdr.list)); + + if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num)) { + preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + } + + /* Set this as a lock to make sure that only one thread is indicating packet. */ + /* pTS->RxIndicateState = RXTS_INDICATE_PROCESSING; */ + + /* Indicate packets */ + /* RT_ASSERT((index<=REORDER_WIN_SIZE), ("RxReorderIndicatePacket(): Rx Reorder buffer full!!\n")); */ + + + /* indicate this recv_frame */ + /* DbgPrint("recv_indicatepkts_in_order, indicate_seq =%d, seq_num =%d\n", precvpriv->indicate_seq, pattrib->seq_num); */ + if (!pattrib->amsdu) { + /* DBG_871X("recv_indicatepkts_in_order, amsdu!= 1, indicate_seq =%d, seq_num =%d\n", preorder_ctrl->indicate_seq, pattrib->seq_num); */ + + if ((padapter->bDriverStopped == false) && + (padapter->bSurpriseRemoved == false)) + rtw_recv_indicatepkt(padapter, prframe);/* indicate this recv_frame */ + + } else if (pattrib->amsdu == 1) { + if (amsdu_to_msdu(padapter, prframe) != _SUCCESS) + rtw_free_recvframe(prframe, &precvpriv->free_recv_queue); + + } else{ + /* error condition; */ + } + + + /* Update local variables. */ + bPktInBuf = false; + + } else{ + bPktInBuf = true; + break; + } + + /* DbgPrint("recv_indicatepkts_in_order():while\n"); */ + + } + + /* spin_unlock(&ppending_recvframe_queue->lock); */ + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */ + + return bPktInBuf; +} + +int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe); +int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe) +{ + int retval = _SUCCESS; + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl; + struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_reoder); + + if (!pattrib->amsdu) { + /* s1. */ + wlanhdr_to_ethhdr(prframe); + + if (pattrib->qos != 1) { + 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_indicatepkt(padapter, prframe); + return _SUCCESS; + + } + + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s pattrib->qos != 1\n", __func__); + #endif + + return _FAIL; + + } + + if (preorder_ctrl->enable == false) { + /* indicate this recv_frame */ + preorder_ctrl->indicate_seq = pattrib->seq_num; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + + rtw_recv_indicatepkt(padapter, prframe); + + preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + + return _SUCCESS; + } + } else if (pattrib->amsdu == 1) { /* temp filter -> means didn't support A-MSDUs in a A-MPDU */ + if (preorder_ctrl->enable == false) { + preorder_ctrl->indicate_seq = pattrib->seq_num; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + + retval = amsdu_to_msdu(padapter, prframe); + + preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + + if (retval != _SUCCESS) { + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __func__); + #endif + } + + 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)) { + pdbgpriv->dbg_rx_ampdu_drop_count++; + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s check_indicate_seq fail\n", __func__); + #endif + goto _err_exit; + } + + + /* s3. Insert all packet into Reorder Queue to maintain its ordering. */ + if (!enqueue_reorder_recvframe(preorder_ctrl, prframe)) { + /* DbgPrint("recv_indicatepkt_reorder, enqueue_reorder_recvframe fail!\n"); */ + /* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */ + /* return _FAIL; */ + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s enqueue_reorder_recvframe fail\n", __func__); + #endif + goto _err_exit; + } + + + /* s4. */ + /* Indication process. */ + /* After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */ + /* with the SeqNum smaller than latest WinStart and buffer other packets. */ + /* */ + /* For Rx Reorder condition: */ + /* 1. All packets with SeqNum smaller than WinStart => Indicate */ + /* 2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */ + /* */ + + /* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */ + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false) == true) { + _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME); + spin_unlock_bh(&ppending_recvframe_queue->lock); + } else{ + spin_unlock_bh(&ppending_recvframe_queue->lock); + 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_handler(void *pcontext) +{ + struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext; + struct adapter *padapter = preorder_ctrl->padapter; + struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + + if (padapter->bDriverStopped || padapter->bSurpriseRemoved) + return; + + /* DBG_871X("+rtw_reordering_ctrl_timeout_handler() =>\n"); */ + + spin_lock_bh(&ppending_recvframe_queue->lock); + + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true) + _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME); + + spin_unlock_bh(&ppending_recvframe_queue->lock); + +} + +int process_recv_indicatepkts(struct adapter *padapter, union recv_frame *prframe); +int process_recv_indicatepkts(struct adapter *padapter, union recv_frame *prframe) +{ + int retval = _SUCCESS; + /* struct recv_priv *precvpriv = &padapter->recvpriv; */ + /* struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; */ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate); + + if (phtpriv->ht_option == true) { /* B/G/N Mode */ + /* prframe->u.hdr.preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */ + + if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) { /* including perform A-MPDU Rx Ordering Buffer Control */ + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s recv_indicatepkt_reorder error!\n", __func__); + #endif + + 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")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __func__); + #endif + 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_indicatepkt(padapter, prframe); + + + } else{ + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n")); + + RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved)); + retval = _FAIL; + return retval; + } + + } + + return retval; + +} + +static int recv_func_prehandle(struct adapter *padapter, union recv_frame *rframe) +{ + int ret = _SUCCESS; + struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + + DBG_COUNTER(padapter->rx_logs.core_rx_pre); + + /* check the frame crtl field and decache */ + ret = validate_recv_frame(padapter, rframe); + if (ret != _SUCCESS) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n")); + rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */ + goto exit; + } + +exit: + return ret; +} + +static int recv_func_posthandle(struct adapter *padapter, union recv_frame *prframe) +{ + int ret = _SUCCESS; + union recv_frame *orig_prframe = prframe; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + + DBG_COUNTER(padapter->rx_logs.core_rx_post); + + prframe = decryptor(padapter, prframe); + if (prframe == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decryptor: drop pkt\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s decryptor: drop pkt\n", __func__); + #endif + ret = _FAIL; + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_err); + goto _recv_data_drop; + } + + prframe = recvframe_chk_defrag(padapter, prframe); + if (prframe == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chk_defrag: drop pkt\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s recvframe_chk_defrag: drop pkt\n", __func__); + #endif + DBG_COUNTER(padapter->rx_logs.core_rx_post_defrag_err); + goto _recv_data_drop; + } + + prframe = portctrl(padapter, prframe); + if (prframe == NULL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("portctrl: drop pkt\n")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s portctrl: drop pkt\n", __func__); + #endif + ret = _FAIL; + DBG_COUNTER(padapter->rx_logs.core_rx_post_portctrl_err); + 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")); + #ifdef DBG_RX_DROP_FRAME + DBG_871X("DBG_RX_DROP_FRAME %s process_recv_indicatepkts fail!\n", __func__); + #endif + rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */ + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_err); + goto _recv_data_drop; + } + +_recv_data_drop: + precvpriv->rx_drop++; + return ret; +} + + +int recv_func(struct adapter *padapter, union recv_frame *rframe); +int recv_func(struct adapter *padapter, union recv_frame *rframe) +{ + int ret; + struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.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) { + union recv_frame *pending_frame; + int cnt = 0; + + while ((pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue))) { + cnt++; + DBG_COUNTER(padapter->rx_logs.core_rx_dequeue); + recv_func_posthandle(padapter, pending_frame); + } + + if (cnt) + DBG_871X(FUNC_ADPT_FMT" dequeue %d from uc_swdec_pending_queue\n", + FUNC_ADPT_ARG(padapter), cnt); + } + + DBG_COUNTER(padapter->rx_logs.core_rx); + ret = recv_func_prehandle(padapter, rframe); + + if (ret == _SUCCESS) { + + /* check if need to enqueue into uc_swdec_pending_queue*/ + if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && + !IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 && + (prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt == true) && + psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK && + !psecuritypriv->busetkipkey) { + DBG_COUNTER(padapter->rx_logs.core_rx_enqueue); + rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue); + /* DBG_871X("%s: no key, enqueue uc_swdec_pending_queue\n", __func__); */ + + if (recvpriv->free_recvframe_cnt < NR_RECVFRAME/4) { + /* to prevent from recvframe starvation, get recvframe from uc_swdec_pending_queue to free_recvframe_cnt */ + rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue); + if (rframe) + goto do_posthandle; + } + goto exit; + } + +do_posthandle: + ret = recv_func_posthandle(padapter, rframe); + } + +exit: + return ret; +} + + +s32 rtw_recv_entry(union recv_frame *precvframe) +{ + struct adapter *padapter; + struct recv_priv *precvpriv; + s32 ret = _SUCCESS; + +/* RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+rtw_recv_entry\n")); */ + + padapter = precvframe->u.hdr.adapter; + + precvpriv = &padapter->recvpriv; + + ret = recv_func(padapter, precvframe); + if (ret == _FAIL) { + RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("rtw_recv_entry: recv_func return fail!!!\n")); + goto _recv_entry_drop; + } + + + precvpriv->rx_pkts++; + + return ret; + +_recv_entry_drop: + + /* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("_recv_entry_drop\n")); */ + + return ret; +} + +void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS) +{ + struct adapter *adapter = (struct adapter *)FunctionContext; + 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 = 5; /* 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; + } + + if (num_signal_strength == 0) { + if (rtw_get_on_cur_ch_time(adapter) == 0 + || jiffies_to_msecs(jiffies - rtw_get_on_cur_ch_time(adapter)) < 2 * adapter->mlmeextpriv.mlmext_info.bcn_interval + ) { + goto set_timer; + } + } + + if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == true + || check_fwstate(&adapter->mlmepriv, _FW_LINKED) == false + ) { + goto set_timer; + } + + /* update value of signal_strength, rssi, signal_qual */ + 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; + + #if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1 + DBG_871X(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u" + ", num_signal_strength:%u, num_signal_qual:%u" + ", on_cur_ch_ms:%d" + "\n" + , FUNC_ADPT_ARG(adapter) + , recvpriv->signal_strength + , recvpriv->rssi + , recvpriv->signal_qual + , num_signal_strength, num_signal_qual + , rtw_get_on_cur_ch_time(adapter) ? jiffies_to_msecs(jiffies - rtw_get_on_cur_ch_time(adapter)) : 0 + ); + #endif + } + +set_timer: + rtw_set_signal_stat_timer(recvpriv); + +} diff --git a/drivers/staging/rtl8723bs/core/rtw_rf.c b/drivers/staging/rtl8723bs/core/rtw_rf.c new file mode 100644 index 0000000..b87ea4e --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_rf.c @@ -0,0 +1,64 @@ +/****************************************************************************** + * + * 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_RF_C_ + +#include <drv_types.h> + + +struct ch_freq { + u32 channel; + u32 frequency; +}; + +static struct ch_freq ch_freq_map[] = { + {1, 2412}, {2, 2417}, {3, 2422}, {4, 2427}, {5, 2432}, + {6, 2437}, {7, 2442}, {8, 2447}, {9, 2452}, {10, 2457}, + {11, 2462}, {12, 2467}, {13, 2472}, {14, 2484}, + /* UNII */ + {36, 5180}, {40, 5200}, {44, 5220}, {48, 5240}, {52, 5260}, + {56, 5280}, {60, 5300}, {64, 5320}, {149, 5745}, {153, 5765}, + {157, 5785}, {161, 5805}, {165, 5825}, {167, 5835}, {169, 5845}, + {171, 5855}, {173, 5865}, + /* HiperLAN2 */ + {100, 5500}, {104, 5520}, {108, 5540}, {112, 5560}, {116, 5580}, + {120, 5600}, {124, 5620}, {128, 5640}, {132, 5660}, {136, 5680}, + {140, 5700}, + /* Japan MMAC */ + {34, 5170}, {38, 5190}, {42, 5210}, {46, 5230}, + /* Japan */ + {184, 4920}, {188, 4940}, {192, 4960}, {196, 4980}, + {208, 5040},/* Japan, means J08 */ + {212, 5060},/* Japan, means J12 */ + {216, 5080},/* Japan, means J16 */ +}; + +static int ch_freq_map_num = (sizeof(ch_freq_map) / sizeof(struct ch_freq)); + +u32 rtw_ch2freq(u32 channel) +{ + u8 i; + u32 freq = 0; + + for (i = 0; i < ch_freq_map_num; i++) { + if (channel == ch_freq_map[i].channel) { + freq = ch_freq_map[i].frequency; + break; + } + } + if (i == ch_freq_map_num) + freq = 2412; + + return freq; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_security.c b/drivers/staging/rtl8723bs/core/rtw_security.c new file mode 100644 index 0000000..e832f16 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_security.c @@ -0,0 +1,2437 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> + +static const char *_security_type_str[] = { + "N/A", + "WEP40", + "TKIP", + "TKIP_WM", + "AES", + "WEP104", + "SMS4", + "WEP_WPA", + "BIP", +}; + +const char *security_type_str(u8 value) +{ + if (value <= _BIP_) + return _security_type_str[value]; + return NULL; +} + +#ifdef DBG_SW_SEC_CNT +#define WEP_SW_ENC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->wep_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->wep_sw_enc_cnt_mc++; \ + else \ + sec->wep_sw_enc_cnt_uc++; + +#define WEP_SW_DEC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->wep_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->wep_sw_dec_cnt_mc++; \ + else \ + sec->wep_sw_dec_cnt_uc++; + +#define TKIP_SW_ENC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->tkip_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->tkip_sw_enc_cnt_mc++; \ + else \ + sec->tkip_sw_enc_cnt_uc++; + +#define TKIP_SW_DEC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->tkip_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->tkip_sw_dec_cnt_mc++; \ + else \ + sec->tkip_sw_dec_cnt_uc++; + +#define AES_SW_ENC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->aes_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->aes_sw_enc_cnt_mc++; \ + else \ + sec->aes_sw_enc_cnt_uc++; + +#define AES_SW_DEC_CNT_INC(sec, ra) \ + if (is_broadcast_mac_addr(ra)) \ + sec->aes_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->aes_sw_dec_cnt_mc++; \ + else \ + sec->aes_sw_dec_cnt_uc++; +#else +#define WEP_SW_ENC_CNT_INC(sec, ra) +#define WEP_SW_DEC_CNT_INC(sec, ra) +#define TKIP_SW_ENC_CNT_INC(sec, ra) +#define TKIP_SW_DEC_CNT_INC(sec, ra) +#define AES_SW_ENC_CNT_INC(sec, ra) +#define AES_SW_DEC_CNT_INC(sec, ra) +#endif /* DBG_SW_SEC_CNT */ + +/* 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 sint bcrc32initialized = 0; +static u32 crc32_table[256]; + + +static u8 crc32_reverseBit(u8 data) +{ + return((u8)((data<<7)&0x80) | ((data<<5)&0x40) | ((data<<3)&0x20) | ((data<<1)&0x10) | ((data>>1)&0x08) | ((data>>3)&0x04) | ((data>>5)&0x02) | ((data>>7)&0x01)); +} + +static void crc32_init(void) +{ + if (bcrc32initialized == 1) + return; + else { + sint i, j; + u32 c; + u8 *p = (u8 *)&c, *p1; + u8 k; + + c = 0x12340000; + + for (i = 0; i < 256; ++i) { + k = crc32_reverseBit((u8)i); + for (c = ((u32)k) << 24, j = 8; j > 0; --j) { + c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1); + } + p1 = (u8 *)&crc32_table[i]; + + p1[0] = crc32_reverseBit(p[3]); + p1[1] = crc32_reverseBit(p[2]); + p1[2] = crc32_reverseBit(p[1]); + p1[3] = crc32_reverseBit(p[0]); + } + bcrc32initialized = 1; + } +} + +static __le32 getcrc32(u8 *buf, sint len) +{ + u8 *p; + u32 crc; + + if (bcrc32initialized == 0) + crc32_init(); + + crc = 0xffffffff; /* preload shift register, per CRC-32 spec */ + + for (p = buf; len > 0; ++p, --len) { + crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8); + } + return cpu_to_le32(~crc); /* transmit complement, per CRC-32 spec */ +} + + +/* + Need to consider the fragment situation +*/ +void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe) +{ /* exclude ICV */ + + unsigned char crc[4]; + struct arc4context mycontext; + + sint curfragnum, length; + u32 keylength; + + u8 *pframe, *payload, *iv; /* wepkey */ + u8 wepkey[16]; + u8 hw_hdr_offset = 0; + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return; + + hw_hdr_offset = TXDESC_OFFSET; + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* start to encrypt each fragment */ + if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) { + keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex]; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + iv = pframe+pattrib->hdrlen; + memcpy(&wepkey[0], iv, 3); + memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); + payload = pframe+pattrib->iv_len+pattrib->hdrlen; + + if ((curfragnum+1) == pattrib->nr_frags) { /* the last fragment */ + + length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + + *((__le32 *)crc) = getcrc32(payload, length); + + arcfour_init(&mycontext, wepkey, 3+keylength); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload+length, crc, 4); + + } else{ + length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + *((__le32 *)crc) = getcrc32(payload, length); + arcfour_init(&mycontext, wepkey, 3+keylength); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload+length, crc, 4); + + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + } + } + + WEP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } +} + +void rtw_wep_decrypt(struct adapter *padapter, u8 *precvframe) +{ + /* exclude ICV */ + u8 crc[4]; + struct arc4context mycontext; + sint length; + u32 keylength; + u8 *pframe, *payload, *iv, wepkey[16]; + u8 keyindex; + struct rx_pkt_attrib *prxattrib = &(((union recv_frame *)precvframe)->u.hdr.attrib); + struct security_priv *psecuritypriv = &padapter->securitypriv; + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + + /* start to decrypt recvframe */ + if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) { + 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 = ((union recv_frame *)precvframe)->u.hdr.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_decrypt: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])); + } + + WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } + return; +} + +/* 3 =====TKIP related ===== */ + +static u32 secmicgetuint32(u8 *p) +/* Convert from Byte[] to Us3232 in a portable way */ +{ + s32 i; + u32 res = 0; + + for (i = 0; i < 4; i++) { + res |= ((u32)(*p++)) << (8*i); + } + + return res; +} + +static void secmicputuint32(u8 *p, u32 val) +/* Convert from Us3232 to Byte[] in a portable way */ +{ + long i; + + for (i = 0; i < 4; i++) { + *p++ = (u8) (val & 0xff); + val >>= 8; + } +} + +static void secmicclear(struct mic_data *pmicdata) +{ +/* Reset the state to the empty message. */ + pmicdata->L = pmicdata->K0; + pmicdata->R = pmicdata->K1; + pmicdata->nBytesInM = 0; + pmicdata->M = 0; +} + +void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key) +{ + /* Set the key */ + pmicdata->K0 = secmicgetuint32(key); + pmicdata->K1 = secmicgetuint32(key + 4); + /* and reset the message */ + secmicclear(pmicdata); +} + +void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b) +{ + /* Append the byte to our word-sized buffer */ + pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM); + pmicdata->nBytesInM++; + /* Process the word if it is full. */ + if (pmicdata->nBytesInM >= 4) { + pmicdata->L ^= pmicdata->M; + pmicdata->R ^= ROL32(pmicdata->L, 17); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ROL32(pmicdata->L, 3); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ROR32(pmicdata->L, 2); + pmicdata->L += pmicdata->R; + /* Clear the buffer */ + pmicdata->M = 0; + pmicdata->nBytesInM = 0; + } +} + +void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes) +{ + /* This is simple */ + while (nbytes > 0) { + rtw_secmicappendbyte(pmicdata, *src++); + nbytes--; + } +} + +void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst) +{ + /* Append the minimum padding */ + rtw_secmicappendbyte(pmicdata, 0x5a); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + /* and then zeroes until the length is a multiple of 4 */ + while (pmicdata->nBytesInM != 0) { + rtw_secmicappendbyte(pmicdata, 0); + } + /* The appendByte function has already computed the result. */ + secmicputuint32(dst, pmicdata->L); + secmicputuint32(dst+4, pmicdata->R); + /* Reset to the empty message. */ + secmicclear(pmicdata); +} + + +void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri) +{ + + struct mic_data micdata; + u8 priority[4] = {0x0, 0x0, 0x0, 0x0}; + + rtw_secmicsetkey(&micdata, key); + priority[0] = pri; + + /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */ + if (header[1]&1) { /* ToDS == 1 */ + rtw_secmicappend(&micdata, &header[16], 6); /* DA */ + if (header[1]&2) /* From Ds == 1 */ + rtw_secmicappend(&micdata, &header[24], 6); + else + rtw_secmicappend(&micdata, &header[10], 6); + } else { /* ToDS == 0 */ + rtw_secmicappend(&micdata, &header[4], 6); /* DA */ + if (header[1]&2) /* From Ds == 1 */ + rtw_secmicappend(&micdata, &header[16], 6); + else + rtw_secmicappend(&micdata, &header[10], 6); + + } + rtw_secmicappend(&micdata, &priority[0], 4); + + + rtw_secmicappend(&micdata, data, data_len); + + rtw_secgetmic(&micdata, mic_code); +} + +/* macros for extraction/creation of unsigned char/unsigned short values */ +#define RotR1(v16) ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15)) +#define Lo8(v16) ((u8)((v16) & 0x00FF)) +#define Hi8(v16) ((u8)(((v16) >> 8) & 0x00FF)) +#define Lo16(v32) ((u16)((v32) & 0xFFFF)) +#define Hi16(v32) ((u16)(((v32) >> 16) & 0xFFFF)) +#define Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8)) + +/* select the Nth 16-bit word of the temporal key unsigned char array TK[] */ +#define TK16(N) Mk16(tk[2*(N)+1], tk[2*(N)]) + +/* S-box lookup: 16 bits --> 16 bits */ +#define _S_(v16) (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)]) + +/* fixed algorithm "parameters" */ +#define PHASE1_LOOP_CNT 8 /* this needs to be "big enough" */ +#define TA_SIZE 6 /* 48-bit transmitter address */ +#define TK_SIZE 16 /* 128-bit temporal key */ +#define P1K_SIZE 10 /* 80-bit Phase1 key */ +#define RC4_KEY_SIZE 16 /* 128-bit RC4KEY (104 bits unknown) */ + + +/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */ +static const unsigned short Sbox1[2][256] = { /* Sbox for hash (can be in ROM) */ +{ + 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154, + 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A, + 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B, + 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B, + 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F, + 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F, + 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5, + 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F, + 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB, + 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397, + 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED, + 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A, + 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194, + 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3, + 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104, + 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D, + 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39, + 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695, + 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83, + 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76, + 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4, + 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B, + 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0, + 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018, + 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751, + 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85, + 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12, + 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9, + 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7, + 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A, + 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8, + 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A, + }, + + + { /* second half of table is unsigned char-reversed version of first! */ + 0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491, + 0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC, + 0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB, + 0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B, + 0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83, + 0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A, + 0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F, + 0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA, + 0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B, + 0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713, + 0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6, + 0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85, + 0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411, + 0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B, + 0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1, + 0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF, + 0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E, + 0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6, + 0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B, + 0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD, + 0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8, + 0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2, + 0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049, + 0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810, + 0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197, + 0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F, + 0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C, + 0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927, + 0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733, + 0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5, + 0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0, + 0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C, + } +}; + + /* +********************************************************************** +* Routine: Phase 1 -- generate P1K, given TA, TK, IV32 +* +* Inputs: +* tk[] = temporal key [128 bits] +* ta[] = transmitter's MAC address [ 48 bits] +* iv32 = upper 32 bits of IV [ 32 bits] +* Output: +* p1k[] = Phase 1 key [ 80 bits] +* +* Note: +* This function only needs to be called every 2**16 packets, +* although in theory it could be called every packet. +* +********************************************************************** +*/ +static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32) +{ + sint 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) +{ + sint i; + u16 PPK[6]; /* temporary key for mixing */ + + /* Note: all adds in the PPK[] equations below are mod 2**16 */ + for (i = 0; i < 5; i++) + PPK[i] = p1k[i]; /* first, copy P1K to PPK */ + + PPK[5] = p1k[4]+iv16; /* next, add in IV16 */ + + /* Bijective non-linear mixing of the 96 bits of PPK[0..5] */ + PPK[0] += _S_(PPK[5] ^ TK16(0)); /* Mix key in each "round" */ + PPK[1] += _S_(PPK[0] ^ TK16(1)); + PPK[2] += _S_(PPK[1] ^ TK16(2)); + PPK[3] += _S_(PPK[2] ^ TK16(3)); + PPK[4] += _S_(PPK[3] ^ TK16(4)); + PPK[5] += _S_(PPK[4] ^ TK16(5)); /* Total # S-box lookups == 6 */ + + /* Final sweep: bijective, "linear". Rotates kill LSB correlations */ + PPK[0] += RotR1(PPK[5] ^ TK16(6)); + PPK[1] += RotR1(PPK[0] ^ TK16(7)); /* Use all of TK[] in Phase2 */ + PPK[2] += RotR1(PPK[1]); + PPK[3] += RotR1(PPK[2]); + PPK[4] += RotR1(PPK[3]); + PPK[5] += RotR1(PPK[4]); + /* Note: At this point, for a given key TK[0..15], the 96-bit output */ + /* value PPK[0..5] is guaranteed to be unique, as a function */ + /* of the 96-bit "input" value {TA, IV32, IV16}. That is, P1K */ + /* is now a keyed permutation of {TA, IV32, IV16}. */ + + /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key */ + rc4key[0] = Hi8(iv16); /* RC4KEY[0..2] is the WEP IV */ + rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys */ + rc4key[2] = Lo8(iv16); + rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1); + + + /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15] (little-endian) */ + for (i = 0; i < 6; i++) { + rc4key[4+2*i] = Lo8(PPK[i]); + rc4key[5+2*i] = Hi8(PPK[i]); + } +} + + +/* The hlen isn't include the IV */ +u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe) +{ /* exclude ICV */ + u16 pnl; + u32 pnh; + u8 rc4key[16]; + u8 ttkey[16]; + u8 crc[4]; + u8 hw_hdr_offset = 0; + struct arc4context mycontext; + sint curfragnum, length; + u32 prwskeylen; + + u8 *pframe, *payload, *iv, *prwskey; + union pn48 dot11txpn; + /* struct sta_info *stainfo; */ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + + hw_hdr_offset = TXDESC_OFFSET; + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* 4 start to encrypt each fragment */ + if (pattrib->encrypt == _TKIP_) { + +/* + if (pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + DBG_871X("%s, call rtw_get_stainfo()\n", __func__); + stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]); + } +*/ + /* if (stainfo!= NULL) */ + { +/* + if (!(stainfo->state &_FW_LINKED)) + { + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state); + return _FAIL; + } +*/ + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo!= NULL!!!\n")); + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else + /* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */ + prwskey = pattrib->dot118021x_UncstKey.skey; + + prwskeylen = 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + iv = pframe+pattrib->hdrlen; + payload = pframe+pattrib->iv_len+pattrib->hdrlen; + + GET_TKIP_PN(iv, dot11txpn); + + pnl = (u16)(dot11txpn.val); + pnh = (u32)(dot11txpn.val>>16); + + phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh); + + phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl); + + if ((curfragnum+1) == pattrib->nr_frags) { /* 4 the last fragment */ + length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len, pattrib->icv_len)); + *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/ + + arcfour_init(&mycontext, rc4key, 16); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload+length, crc, 4); + + } else { + length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/ + arcfour_init(&mycontext, rc4key, 16); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload+length, crc, 4); + + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + } + } + + TKIP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } +/* + else { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo == NULL!!!\n")); + DBG_871X("%s, psta ==NUL\n", __func__); + res = _FAIL; + } +*/ + + } + return res; +} + + +/* The hlen isn't include the IV */ +u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe) +{ /* exclude ICV */ + u16 pnl; + u32 pnh; + u8 rc4key[16]; + u8 ttkey[16]; + u8 crc[4]; + struct arc4context mycontext; + sint length; + u32 prwskeylen; + + u8 *pframe, *payload, *iv, *prwskey; + union pn48 dot11txpn; + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; +/* struct recv_priv *precvpriv =&padapter->recvpriv; */ + u32 res = _SUCCESS; + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + + /* 4 start to decrypt recvframe */ + if (prxattrib->encrypt == _TKIP_) { + stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]); + if (stainfo != NULL) { + if (IS_MCAST(prxattrib->ra)) { + static unsigned long start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + if (psecuritypriv->binstallGrpkey == false) { + res = _FAIL; + + if (start == 0) + start = jiffies; + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (jiffies_to_msecs(jiffies - start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = jiffies; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + /* DBG_871X("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n"); */ + /* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */ + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + prwskeylen = 16; + } else{ + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + prwskeylen = 16; + } + + iv = pframe+prxattrib->hdrlen; + payload = pframe+prxattrib->iv_len+prxattrib->hdrlen; + length = ((union recv_frame *)precvframe)->u.hdr.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_decrypt: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; + } + + TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo == NULL!!!\n")); + res = _FAIL; + } + + } +exit: + return res; + +} + + +/* 3 =====AES related ===== */ + + + +#define MAX_MSG_SIZE 2048 +/*****************************/ +/******** SBOX Table *********/ +/*****************************/ + + static u8 sbox_table[256] = { + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, + 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, + 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, + 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, + 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, + 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, + 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, + 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, + 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, + 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, + 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, + 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, + 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, + 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, + 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, + 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, + 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 + }; + +/*****************************/ +/**** Function Prototypes ****/ +/*****************************/ + +static void bitwise_xor(u8 *ina, u8 *inb, u8 *out); +static void construct_mic_iv( + u8 *mic_header1, + sint qc_exists, + sint a4_exists, + u8 *mpdu, + uint payload_length, + u8 *pn_vector, + uint frtype +);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void construct_mic_header1( + u8 *mic_header1, + sint header_length, + u8 *mpdu, + uint frtype +);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void construct_mic_header2( + u8 *mic_header2, + u8 *mpdu, + sint a4_exists, + sint qc_exists +); +static void construct_ctr_preload( + u8 *ctr_preload, + sint a4_exists, + sint qc_exists, + u8 *mpdu, + u8 *pn_vector, + sint c, + uint frtype +);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void xor_128(u8 *a, u8 *b, u8 *out); +static void xor_32(u8 *a, u8 *b, u8 *out); +static u8 sbox(u8 a); +static void next_key(u8 *key, sint round); +static void byte_sub(u8 *in, u8 *out); +static void shift_row(u8 *in, u8 *out); +static void mix_column(u8 *in, u8 *out); +static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext); + + +/****************************************/ +/* aes128k128d() */ +/* Performs a 128 bit AES encrypt with */ +/* 128 bit data. */ +/****************************************/ +static void xor_128(u8 *a, u8 *b, u8 *out) +{ + sint i; + + for (i = 0; i < 16; i++) { + out[i] = a[i] ^ b[i]; + } +} + + +static void xor_32(u8 *a, u8 *b, u8 *out) +{ + sint i; + + for (i = 0; i < 4; i++) { + out[i] = a[i] ^ b[i]; + } +} + + +static u8 sbox(u8 a) +{ + return sbox_table[(sint)a]; +} + + +static void next_key(u8 *key, sint 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) +{ + sint 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) +{ + sint 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) +{ + sint round; + sint 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 */ +/* Baron think the function is construct CCM */ +/* nonce */ +/************************************************/ +static void construct_mic_iv( + u8 *mic_iv, + sint qc_exists, + sint a4_exists, + u8 *mpdu, + uint payload_length, + u8 *pn_vector, + uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + sint 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; + + /* 802.11w management frame should set management bit(4) */ + if (frtype == WIFI_MGT_TYPE) + mic_iv[1] |= BIT(4); + + for (i = 2; i < 8; i++) + mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */ + #ifdef CONSISTENT_PN_ORDER + for (i = 8; i < 14; i++) + mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */ + #else + for (i = 8; i < 14; i++) + mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */ + #endif + 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. */ +/* Build AAD SC, A1, A2 */ +/************************************************/ +static void construct_mic_header1( + u8 *mic_header1, + sint header_length, + u8 *mpdu, + uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + mic_header1[0] = (u8)((header_length - 2) / 256); + mic_header1[1] = (u8)((header_length - 2) % 256); + + /* 802.11w management frame don't AND subtype bits 4, 5, 6 of frame control field */ + if (frtype == WIFI_MGT_TYPE) + mic_header1[2] = mpdu[0]; + else + 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, + sint a4_exists, + sint qc_exists +) +{ + sint 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. */ +/* Baron think the function is construct CCM */ +/* nonce */ +/************************************************/ +static void construct_ctr_preload( + u8 *ctr_preload, + sint a4_exists, + sint qc_exists, + u8 *mpdu, + u8 *pn_vector, + sint c, + uint frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + sint 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; + + /* 802.11w management frame should set management bit(4) */ + if (frtype == WIFI_MGT_TYPE) + ctr_preload[1] |= BIT(4); + + for (i = 2; i < 8; i++) + ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */ +#ifdef CONSISTENT_PN_ORDER + for (i = 8; i < 14; i++) + ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */ +#else + for (i = 8; i < 14; i++) + ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */ +#endif + 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) +{ + sint i; + + for (i = 0; i < 16; i++) { + out[i] = ina[i] ^ inb[i]; + } +} + + +static sint aes_cipher(u8 *key, uint hdrlen, + u8 *pframe, uint plen) +{ + uint qc_exists, a4_exists, i, j, payload_remainder, + num_blocks, payload_index; + + u8 pn_vector[6]; + u8 mic_iv[16]; + u8 mic_header1[16]; + u8 mic_header2[16]; + u8 ctr_preload[16]; + + /* Intermediate Buffers */ + u8 chain_buffer[16]; + u8 aes_out[16]; + u8 padded_buffer[16]; + u8 mic[8]; + uint frtype = GetFrameType(pframe); + uint frsubtype = GetFrameSubType(pframe); + + frsubtype = frsubtype>>4; + + + memset((void *)mic_iv, 0, 16); + memset((void *)mic_header1, 0, 16); + memset((void *)mic_header2, 0, 16); + memset((void *)ctr_preload, 0, 16); + memset((void *)chain_buffer, 0, 16); + memset((void *)aes_out, 0, 16); + memset((void *)padded_buffer, 0, 16); + + if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN)) + a4_exists = 0; + else + a4_exists = 1; + + if (((frtype|frsubtype) == WIFI_DATA_CFACK) || + ((frtype|frsubtype) == WIFI_DATA_CFPOLL) || + ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != WLAN_HDR_A3_QOS_LEN) + hdrlen += 2; + + } else if ((frtype == WIFI_DATA) && /* add for CONFIG_IEEE80211W, none 11w also can use */ + ((frsubtype == 0x08) || + (frsubtype == 0x09) || + (frsubtype == 0x0a) || + (frsubtype == 0x0b))) { + if (hdrlen != WLAN_HDR_A3_QOS_LEN) + hdrlen += 2; + + qc_exists = 1; + } else + qc_exists = 0; + + pn_vector[0] = pframe[hdrlen]; + pn_vector[1] = pframe[hdrlen+1]; + pn_vector[2] = pframe[hdrlen+4]; + pn_vector[3] = pframe[hdrlen+5]; + pn_vector[4] = pframe[hdrlen+6]; + pn_vector[5] = pframe[hdrlen+7]; + + construct_mic_iv( + mic_iv, + qc_exists, + a4_exists, + pframe, /* message, */ + plen, + pn_vector, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + construct_mic_header1( + mic_header1, + hdrlen, + pframe, /* message */ + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + construct_mic_header2( + mic_header2, + pframe, /* message, */ + a4_exists, + qc_exists + ); + + + payload_remainder = plen % 16; + num_blocks = plen / 16; + + /* Find start of payload */ + payload_index = (hdrlen + 8); + + /* Calculate MIC */ + aes128k128d(key, mic_iv, aes_out); + bitwise_xor(aes_out, mic_header1, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + bitwise_xor(aes_out, mic_header2, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + for (i = 0; i < num_blocks; i++) { + bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */ + + payload_index += 16; + aes128k128d(key, chain_buffer, aes_out); + } + + /* Add on the final payload block if it needs padding */ + if (payload_remainder > 0) { + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) { + padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */ + } + bitwise_xor(aes_out, padded_buffer, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + } + + for (j = 0 ; j < 8; j++) + mic[j] = aes_out[j]; + + /* Insert MIC into payload */ + for (j = 0; j < 8; j++) + pframe[payload_index+j] = mic[j]; /* message[payload_index+j] = mic[j]; */ + + payload_index = hdrlen + 8; + for (i = 0; i < num_blocks; i++) { + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, /* message, */ + pn_vector, + i+1, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */ + for (j = 0; j < 16; j++) + pframe[payload_index++] = chain_buffer[j];/* 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, + pframe, /* message, */ + pn_vector, + num_blocks+1, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) + padded_buffer[j] = pframe[payload_index+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++) + pframe[payload_index++] = chain_buffer[j];/* 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, + pframe, /* message, */ + pn_vector, + 0, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < 8; j++) + padded_buffer[j] = pframe[j+hdrlen+8+plen];/* padded_buffer[j] = message[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];/* for (j = 0; j<8;j++) message[payload_index++] = chain_buffer[j]; */ + + return _SUCCESS; +} + +u32 rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe) +{ /* exclude ICV */ + + + /*static*/ +/* unsigned char message[MAX_MSG_SIZE]; */ + + /* Intermediate Buffers */ + sint curfragnum, length; + u32 prwskeylen; + u8 *pframe, *prwskey; /* *payload,*iv */ + u8 hw_hdr_offset = 0; + /* struct sta_info *stainfo = NULL; */ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + +/* uint offset = 0; */ + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + + hw_hdr_offset = TXDESC_OFFSET; + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* 4 start to encrypt each fragment */ + if ((pattrib->encrypt == _AES_)) { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo!= NULL!!!\n")); + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else + /* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */ + prwskey = pattrib->dot118021x_UncstKey.skey; + + prwskeylen = 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + if ((curfragnum+1) == pattrib->nr_frags) { /* 4 the last fragment */ + length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + + aes_cipher(prwskey, pattrib->hdrlen, pframe, length); + } else { + length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len; + + aes_cipher(prwskey, pattrib->hdrlen, pframe, length); + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + } + } + + AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } + return res; +} + +static sint 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; + sint res = _SUCCESS; + u8 pn_vector[6]; + u8 mic_iv[16]; + u8 mic_header1[16]; + u8 mic_header2[16]; + u8 ctr_preload[16]; + + /* Intermediate Buffers */ + u8 chain_buffer[16]; + u8 aes_out[16]; + u8 padded_buffer[16]; + u8 mic[8]; + + +/* uint offset = 0; */ + uint frtype = GetFrameType(pframe); + uint frsubtype = GetFrameSubType(pframe); + + frsubtype = frsubtype>>4; + + + memset((void *)mic_iv, 0, 16); + memset((void *)mic_header1, 0, 16); + memset((void *)mic_header2, 0, 16); + memset((void *)ctr_preload, 0, 16); + memset((void *)chain_buffer, 0, 16); + memset((void *)aes_out, 0, 16); + memset((void *)padded_buffer, 0, 16); + + /* start to decrypt the payload */ + + num_blocks = (plen-8) / 16; /* plen including LLC, payload_length and mic) */ + + payload_remainder = (plen-8) % 16; + + pn_vector[0] = pframe[hdrlen]; + pn_vector[1] = pframe[hdrlen+1]; + pn_vector[2] = pframe[hdrlen+4]; + pn_vector[3] = pframe[hdrlen+5]; + pn_vector[4] = pframe[hdrlen+6]; + pn_vector[5] = pframe[hdrlen+7]; + + if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN)) + a4_exists = 0; + else + a4_exists = 1; + + if (((frtype|frsubtype) == WIFI_DATA_CFACK) || + ((frtype|frsubtype) == WIFI_DATA_CFPOLL) || + ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != WLAN_HDR_A3_QOS_LEN) { + hdrlen += 2; + } + } else if ((frtype == WIFI_DATA) && /* only for data packet . add for CONFIG_IEEE80211W, none 11w also can use */ + ((frsubtype == 0x08) || + (frsubtype == 0x09) || + (frsubtype == 0x0a) || + (frsubtype == 0x0b))) { + if (hdrlen != WLAN_HDR_A3_QOS_LEN) { + hdrlen += 2; + } + qc_exists = 1; + } else + qc_exists = 0; + + + /* now, decrypt pframe with hdrlen offset and plen long */ + + payload_index = hdrlen + 8; /* 8 is for extiv */ + + for (i = 0; i < num_blocks; i++) { + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, + pn_vector, + i+1, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + 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, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + 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((void *)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, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + construct_mic_header1( + mic_header1, + hdrlen, + message, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + 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, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + 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, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + 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, + frtype + ); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + 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_871X("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n", + i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]); + res = _FAIL; + } + } + return res; +} + +u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe) +{ /* exclude ICV */ + + + /*static*/ +/* unsigned char message[MAX_MSG_SIZE]; */ + + + /* Intermediate Buffers */ + + + sint length; + u8 *pframe, *prwskey; /* *payload,*iv */ + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; +/* struct recv_priv *precvpriv =&padapter->recvpriv; */ + u32 res = _SUCCESS; + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + /* 4 start to encrypt each fragment */ + if ((prxattrib->encrypt == _AES_)) { + + stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]); + if (stainfo != NULL) { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n")); + + if (IS_MCAST(prxattrib->ra)) { + static unsigned long start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + /* DBG_871X("rx bc/mc packets, to perform sw rtw_aes_decrypt\n"); */ + /* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */ + if (psecuritypriv->binstallGrpkey == false) { + res = _FAIL; + + if (start == 0) + start = jiffies; + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (jiffies_to_msecs(jiffies - start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = jiffies; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) { + DBG_871X("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 = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len; + + res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length); + + AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo == NULL!!!\n")); + res = _FAIL; + } + } +exit: + return res; +} + +u32 rtw_BIP_verify(struct adapter *padapter, u8 *precvframe) +{ + struct rx_pkt_attrib *pattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + u8 *pframe; + u8 *BIP_AAD, *p; + u32 res = _FAIL; + uint len, ori_len; + struct ieee80211_hdr *pwlanhdr; + u8 mic[16]; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + __le16 le_tmp; + __le64 le_tmp64; + + ori_len = pattrib->pkt_len-WLAN_HDR_A3_LEN+BIP_AAD_SIZE; + BIP_AAD = rtw_zmalloc(ori_len); + + if (BIP_AAD == NULL) { + DBG_871X("BIP AAD allocate fail\n"); + return _FAIL; + } + /* PKT start */ + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + /* mapping to wlan header */ + pwlanhdr = (struct ieee80211_hdr *)pframe; + /* save the frame body + MME */ + memcpy(BIP_AAD+BIP_AAD_SIZE, pframe+WLAN_HDR_A3_LEN, pattrib->pkt_len-WLAN_HDR_A3_LEN); + /* find MME IE pointer */ + p = rtw_get_ie(BIP_AAD+BIP_AAD_SIZE, _MME_IE_, &len, pattrib->pkt_len-WLAN_HDR_A3_LEN); + /* Baron */ + if (p) { + u16 keyid = 0; + u64 temp_ipn = 0; + /* save packet number */ + memcpy(&le_tmp64, p+4, 6); + temp_ipn = le64_to_cpu(le_tmp64); + /* BIP packet number should bigger than previous BIP packet */ + if (temp_ipn <= pmlmeext->mgnt_80211w_IPN_rx) { + DBG_871X("replay BIP packet\n"); + goto BIP_exit; + } + /* copy key index */ + memcpy(&le_tmp, p+2, 2); + keyid = le16_to_cpu(le_tmp); + if (keyid != padapter->securitypriv.dot11wBIPKeyid) { + DBG_871X("BIP key index error!\n"); + goto BIP_exit; + } + /* clear the MIC field of MME to zero */ + memset(p+2+len-8, 0, 8); + + /* conscruct AAD, copy frame control field */ + memcpy(BIP_AAD, &pwlanhdr->frame_control, 2); + ClearRetry(BIP_AAD); + ClearPwrMgt(BIP_AAD); + ClearMData(BIP_AAD); + /* conscruct AAD, copy address 1 to address 3 */ + memcpy(BIP_AAD+2, pwlanhdr->addr1, 18); + + if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey + , BIP_AAD, ori_len, mic)) + goto BIP_exit; + + /* MIC field should be last 8 bytes of packet (packet without FCS) */ + if (!memcmp(mic, pframe+pattrib->pkt_len-8, 8)) { + pmlmeext->mgnt_80211w_IPN_rx = temp_ipn; + res = _SUCCESS; + } else + DBG_871X("BIP MIC error!\n"); + + } else + res = RTW_RX_HANDLED; +BIP_exit: + + kfree(BIP_AAD); + return res; +} + +/* AES tables*/ +const u32 Te0[256] = { + 0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU, + 0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U, + 0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU, + 0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU, + 0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U, + 0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU, + 0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU, + 0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU, + 0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU, + 0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU, + 0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U, + 0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU, + 0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU, + 0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U, + 0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU, + 0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU, + 0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU, + 0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU, + 0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU, + 0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U, + 0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU, + 0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU, + 0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU, + 0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU, + 0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U, + 0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U, + 0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U, + 0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U, + 0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU, + 0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U, + 0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U, + 0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU, + 0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU, + 0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U, + 0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U, + 0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U, + 0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU, + 0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U, + 0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU, + 0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U, + 0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU, + 0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U, + 0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U, + 0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU, + 0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U, + 0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U, + 0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U, + 0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U, + 0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U, + 0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U, + 0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U, + 0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U, + 0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU, + 0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U, + 0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U, + 0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U, + 0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U, + 0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U, + 0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U, + 0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU, + 0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U, + 0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U, + 0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U, + 0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU, +}; +const u32 Td0[256] = { + 0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U, + 0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U, + 0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U, + 0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU, + 0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U, + 0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U, + 0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU, + 0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U, + 0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU, + 0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U, + 0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U, + 0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U, + 0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U, + 0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU, + 0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U, + 0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU, + 0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U, + 0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU, + 0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U, + 0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U, + 0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U, + 0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU, + 0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U, + 0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU, + 0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U, + 0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU, + 0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U, + 0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU, + 0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU, + 0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U, + 0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU, + 0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U, + 0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU, + 0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U, + 0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U, + 0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U, + 0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU, + 0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U, + 0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U, + 0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU, + 0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U, + 0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U, + 0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U, + 0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U, + 0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U, + 0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU, + 0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U, + 0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U, + 0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U, + 0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U, + 0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U, + 0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU, + 0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU, + 0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU, + 0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU, + 0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U, + 0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U, + 0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU, + 0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU, + 0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U, + 0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU, + 0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U, + 0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U, + 0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U, +}; +const u8 Td4s[256] = { + 0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U, + 0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU, + 0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U, + 0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU, + 0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU, + 0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU, + 0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U, + 0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U, + 0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U, + 0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U, + 0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU, + 0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U, + 0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU, + 0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U, + 0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U, + 0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU, + 0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU, + 0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U, + 0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U, + 0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU, + 0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U, + 0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU, + 0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U, + 0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U, + 0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U, + 0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU, + 0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU, + 0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU, + 0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U, + 0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U, + 0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U, + 0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU, +}; +const u8 rcons[] = { + 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36 + /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */ +}; + +/** + * Expand the cipher key into the encryption key schedule. + * + * @return the number of rounds for the given cipher key size. + */ +static void rijndaelKeySetupEnc(u32 rk[/*44*/], const u8 cipherKey[]) +{ + int i; + u32 temp; + + rk[0] = GETU32(cipherKey); + rk[1] = GETU32(cipherKey + 4); + rk[2] = GETU32(cipherKey + 8); + rk[3] = GETU32(cipherKey + 12); + for (i = 0; i < 10; i++) { + temp = rk[3]; + rk[4] = rk[0] ^ + TE421(temp) ^ TE432(temp) ^ TE443(temp) ^ TE414(temp) ^ + RCON(i); + rk[5] = rk[1] ^ rk[4]; + rk[6] = rk[2] ^ rk[5]; + rk[7] = rk[3] ^ rk[6]; + rk += 4; + } +} + +static void rijndaelEncrypt(u32 rk[/*44*/], u8 pt[16], u8 ct[16]) +{ + u32 s0, s1, s2, s3, t0, t1, t2, t3; + int Nr = 10; + int r; + + /* + * map byte array block to cipher state + * and add initial round key: + */ + s0 = GETU32(pt) ^ rk[0]; + s1 = GETU32(pt + 4) ^ rk[1]; + s2 = GETU32(pt + 8) ^ rk[2]; + s3 = GETU32(pt + 12) ^ rk[3]; + +#define ROUND(i, d, s) \ +d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \ +d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \ +d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \ +d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3] + + /* Nr - 1 full rounds: */ + r = Nr >> 1; + for (;;) { + ROUND(1, t, s); + rk += 8; + if (--r == 0) + break; + ROUND(0, s, t); + } + +#undef ROUND + + /* + * apply last round and + * map cipher state to byte array block: + */ + s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0]; + PUTU32(ct, s0); + s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1]; + PUTU32(ct + 4, s1); + s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2]; + PUTU32(ct + 8, s2); + s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3]; + PUTU32(ct + 12, s3); +} + +static void *aes_encrypt_init(u8 *key, size_t len) +{ + u32 *rk; + if (len != 16) + return NULL; + rk = (u32 *)rtw_malloc(AES_PRIV_SIZE); + if (rk == NULL) + return NULL; + rijndaelKeySetupEnc(rk, key); + return rk; +} + +static void aes_128_encrypt(void *ctx, u8 *plain, u8 *crypt) +{ + rijndaelEncrypt(ctx, plain, crypt); +} + + +static void gf_mulx(u8 *pad) +{ + int i, carry; + + carry = pad[0] & 0x80; + for (i = 0; i < AES_BLOCK_SIZE - 1; i++) + pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7); + + pad[AES_BLOCK_SIZE - 1] <<= 1; + if (carry) + pad[AES_BLOCK_SIZE - 1] ^= 0x87; +} + +static void aes_encrypt_deinit(void *ctx) +{ + memset(ctx, 0, AES_PRIV_SIZE); + kfree(ctx); +} + + +/** + * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128 + * @key: 128-bit key for the hash operation + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + */ +static int omac1_aes_128_vector(u8 *key, size_t num_elem, + u8 *addr[], size_t *len, u8 *mac) +{ + void *ctx; + u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE]; + u8 *pos, *end; + size_t i, e, left, total_len; + + ctx = aes_encrypt_init(key, 16); + if (ctx == NULL) + return -1; + memset(cbc, 0, AES_BLOCK_SIZE); + + total_len = 0; + for (e = 0; e < num_elem; e++) + total_len += len[e]; + left = total_len; + + e = 0; + pos = addr[0]; + end = pos + len[0]; + + while (left >= AES_BLOCK_SIZE) { + for (i = 0; i < AES_BLOCK_SIZE; i++) { + cbc[i] ^= *pos++; + if (pos >= end) { + e++; + pos = addr[e]; + end = pos + len[e]; + } + } + if (left > AES_BLOCK_SIZE) + aes_128_encrypt(ctx, cbc, cbc); + left -= AES_BLOCK_SIZE; + } + + memset(pad, 0, AES_BLOCK_SIZE); + aes_128_encrypt(ctx, pad, pad); + gf_mulx(pad); + + if (left || total_len == 0) { + for (i = 0; i < left; i++) { + cbc[i] ^= *pos++; + if (pos >= end) { + e++; + pos = addr[e]; + end = pos + len[e]; + } + } + cbc[left] ^= 0x80; + gf_mulx(pad); + } + + for (i = 0; i < AES_BLOCK_SIZE; i++) + pad[i] ^= cbc[i]; + aes_128_encrypt(ctx, pad, mac); + aes_encrypt_deinit(ctx); + return 0; +} + + +/** + * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC) + * @key: 128-bit key for the hash operation + * @data: Data buffer for which a MAC is determined + * @data_len: Length of data buffer in bytes + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + * modify for CONFIG_IEEE80211W */ +int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac) +{ + return omac1_aes_128_vector(key, 1, &data, &data_len, mac); +} + +/* Restore HW wep key setting according to key_mask */ +void rtw_sec_restore_wep_key(struct adapter *adapter) +{ + struct security_priv *securitypriv = &(adapter->securitypriv); + sint keyid; + + if ((_WEP40_ == securitypriv->dot11PrivacyAlgrthm) || (_WEP104_ == securitypriv->dot11PrivacyAlgrthm)) { + for (keyid = 0; keyid < 4; keyid++) { + if (securitypriv->key_mask & BIT(keyid)) { + if (keyid == securitypriv->dot11PrivacyKeyIndex) + rtw_set_key(adapter, securitypriv, keyid, 1, false); + else + rtw_set_key(adapter, securitypriv, keyid, 0, false); + } + } + } +} + +u8 rtw_handle_tkip_countermeasure(struct adapter *adapter, const char *caller) +{ + struct security_priv *securitypriv = &(adapter->securitypriv); + u8 status = _SUCCESS; + + if (securitypriv->btkip_countermeasure == true) { + unsigned long passing_ms = jiffies_to_msecs(jiffies - securitypriv->btkip_countermeasure_time); + if (passing_ms > 60*1000) { + DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%lus > 60s\n", + caller, ADPT_ARG(adapter), passing_ms/1000); + securitypriv->btkip_countermeasure = false; + securitypriv->btkip_countermeasure_time = 0; + } else { + DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%lus < 60s\n", + caller, ADPT_ARG(adapter), passing_ms/1000); + status = _FAIL; + } + } + + return status; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c new file mode 100644 index 0000000..cb43ec9 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c @@ -0,0 +1,641 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> + +void _rtw_init_stainfo(struct sta_info *psta); +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); + /* INIT_LIST_HEAD(&psta->asoc_list); */ + /* INIT_LIST_HEAD(&psta->sleep_list); */ + /* INIT_LIST_HEAD(&psta->wakeup_list); */ + + _rtw_init_queue(&psta->sleep_q); + psta->sleepq_len = 0; + + _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); + _rtw_init_sta_recv_priv(&psta->sta_recvpriv); + + 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->under_exist_checking = 0; + + psta->keep_alive_trycnt = 0; +} + +u32 _rtw_init_sta_priv(struct sta_priv *pstapriv) +{ + struct sta_info *psta; + s32 i; + + pstapriv->pallocated_stainfo_buf = vzalloc(sizeof(struct sta_info) * NUM_STA+4); + + if (!pstapriv->pallocated_stainfo_buf) + return _FAIL; + + pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 - + ((SIZE_PTR)(pstapriv->pallocated_stainfo_buf) & 3); + + _rtw_init_queue(&pstapriv->free_sta_queue); + + spin_lock_init(&pstapriv->sta_hash_lock); + + /* _rtw_init_queue(&pstapriv->asoc_q); */ + pstapriv->asoc_sta_count = 0; + _rtw_init_queue(&pstapriv->sleep_q); + _rtw_init_queue(&pstapriv->wakeup_q); + + psta = (struct sta_info *)(pstapriv->pstainfo_buf); + + + for (i = 0; i < NUM_STA; i++) { + _rtw_init_stainfo(psta); + + INIT_LIST_HEAD(&(pstapriv->sta_hash[i])); + + list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue)); + + psta++; + } + + pstapriv->sta_dz_bitmap = 0; + pstapriv->tim_bitmap = 0; + + INIT_LIST_HEAD(&pstapriv->asoc_list); + INIT_LIST_HEAD(&pstapriv->auth_list); + spin_lock_init(&pstapriv->asoc_list_lock); + spin_lock_init(&pstapriv->auth_list_lock); + pstapriv->asoc_list_cnt = 0; + pstapriv->auth_list_cnt = 0; + + pstapriv->auth_to = 3; /* 3*2 = 6 sec */ + pstapriv->assoc_to = 3; + pstapriv->expire_to = 3; /* 3*2 = 6 sec */ + pstapriv->max_num_sta = NUM_STA; + return _SUCCESS; +} + +inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta) +{ + int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info); + + if (!stainfo_offset_valid(offset)) + DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset); + + return offset; +} + +inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset) +{ + if (!stainfo_offset_valid(offset)) + DBG_871X("%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 kfree_all_stainfo(struct sta_priv *pstapriv); +void kfree_all_stainfo(struct sta_priv *pstapriv) +{ + struct list_head *plist, *phead; + struct sta_info *psta = NULL; + + spin_lock_bh(&pstapriv->sta_hash_lock); + + phead = get_list_head(&pstapriv->free_sta_queue); + plist = get_next(phead); + + while (phead != plist) { + psta = LIST_CONTAINOR(plist, struct sta_info, list); + plist = get_next(plist); + } + + spin_unlock_bh(&pstapriv->sta_hash_lock); +} + +void kfree_sta_priv_lock(struct sta_priv *pstapriv); +void kfree_sta_priv_lock(struct sta_priv *pstapriv) +{ + kfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */ +} + +u32 _rtw_free_sta_priv(struct sta_priv *pstapriv) +{ + struct list_head *phead, *plist; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + int index; + + if (pstapriv) { + + /*delete all reordering_ctrl_timer */ + spin_lock_bh(&pstapriv->sta_hash_lock); + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while (phead != plist) { + int i; + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + + 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); + /*===============================*/ + + kfree_sta_priv_lock(pstapriv); + + if (pstapriv->pallocated_stainfo_buf) + vfree(pstapriv->pallocated_stainfo_buf); + + } + return _SUCCESS; +} + +/* struct sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr) */ +struct sta_info *rtw_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr) +{ + uint tmp_aid; + s32 index; + struct list_head *phash_list; + struct sta_info *psta; + struct __queue *pfree_sta_queue; + struct recv_reorder_ctrl *preorder_ctrl; + int i = 0; + u16 wRxSeqInitialValue = 0xffff; + + pfree_sta_queue = &pstapriv->free_sta_queue; + + /* spin_lock_bh(&(pfree_sta_queue->lock)); */ + spin_lock_bh(&(pstapriv->sta_hash_lock)); + if (list_empty(&pfree_sta_queue->queue)) { + /* spin_unlock_bh(&(pfree_sta_queue->lock)); */ + spin_unlock_bh(&(pstapriv->sta_hash_lock)); + psta = NULL; + return psta; + } else{ + psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list); + + list_del_init(&(psta->list)); + + /* spin_unlock_bh(&(pfree_sta_queue->lock)); */ + + 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_stainfo: index = %x", index)); + + if (index >= NUM_STA) { + RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("ERROR => rtw_alloc_stainfo: index >= NUM_STA")); + spin_unlock_bh(&(pstapriv->sta_hash_lock)); + psta = NULL; + goto exit; + } + phash_list = &(pstapriv->sta_hash[index]); + + /* spin_lock_bh(&(pstapriv->sta_hash_lock)); */ + + list_add_tail(&psta->hash_list, phash_list); + + pstapriv->asoc_sta_count++; + + /* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */ + +/* Commented by Albert 2009/08/13 */ +/* For the SMC router, the sequence number of first packet of WPS handshake will be 0. */ +/* In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable. */ +/* So, we initialize the tid_rxseq variable as the 0xffff. */ + + for (i = 0; i < 16; i++) { + memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2); + } + + RT_TRACE( + _module_rtl871x_sta_mgt_c_, + _drv_info_, ( + "alloc number_%d stainfo with hwaddr = %x %x %x %x %x %x \n", + pstapriv->asoc_sta_count, + hwaddr[0], + hwaddr[1], + hwaddr[2], + hwaddr[3], + hwaddr[4], + hwaddr[5] + ) + ); + + init_addba_retry_timer(pstapriv->padapter, psta); + + /* for A-MPDU Rx reordering buffer control */ + for (i = 0; i < 16 ; i++) { + preorder_ctrl = &psta->recvreorder_ctrl[i]; + + preorder_ctrl->padapter = pstapriv->padapter; + + preorder_ctrl->enable = false; + + preorder_ctrl->indicate_seq = 0xffff; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq); + #endif + preorder_ctrl->wend_b = 0xffff; + /* preorder_ctrl->wsize_b = (NR_RECVBUFF-2); */ + preorder_ctrl->wsize_b = 64;/* 64; */ + + _rtw_init_queue(&preorder_ctrl->pending_recvframe_queue); + + rtw_init_recv_timer(preorder_ctrl); + } + + + /* init for DM */ + psta->rssi_stat.UndecoratedSmoothedPWDB = (-1); + psta->rssi_stat.UndecoratedSmoothedCCK = (-1); + + /* init for the sequence number of received management frame */ + psta->RxMgmtFrameSeqNum = 0xffff; + spin_unlock_bh(&(pstapriv->sta_hash_lock)); + /* alloc mac id for non-bc/mc station, */ + rtw_alloc_macid(pstapriv->padapter, psta); + + } + +exit: + + + return psta; +} + +/* using pstapriv->sta_hash_lock to protect */ +u32 rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta) +{ + int i; + struct __queue *pfree_sta_queue; + struct recv_reorder_ctrl *preorder_ctrl; + struct sta_xmit_priv *pstaxmitpriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct hw_xmit *phwxmit; + + 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; + + /* list_del_init(&psta->sleep_list); */ + + /* list_del_init(&psta->wakeup_list); */ + + spin_lock_bh(&pxmitpriv->lock); + + rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q); + psta->sleepq_len = 0; + + /* vo */ + /* spin_lock_bh(&(pxmitpriv->vo_pending.lock)); */ + rtw_free_xmitframe_queue(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; + /* spin_unlock_bh(&(pxmitpriv->vo_pending.lock)); */ + + /* vi */ + /* spin_lock_bh(&(pxmitpriv->vi_pending.lock)); */ + rtw_free_xmitframe_queue(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; + /* spin_unlock_bh(&(pxmitpriv->vi_pending.lock)); */ + + /* be */ + /* spin_lock_bh(&(pxmitpriv->be_pending.lock)); */ + rtw_free_xmitframe_queue(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; + /* spin_unlock_bh(&(pxmitpriv->be_pending.lock)); */ + + /* bk */ + /* spin_lock_bh(&(pxmitpriv->bk_pending.lock)); */ + rtw_free_xmitframe_queue(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->bk_pending.lock)); */ + + 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_priv(&psta->sta_xmitpriv); */ + /* _rtw_init_sta_recv_priv(&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; + union recv_frame *prframe; + struct __queue *ppending_recvframe_queue; + struct __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 = get_next(phead); + + while (!list_empty(phead)) { + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + + plist = get_next(plist); + + list_del_init(&(prframe->u.hdr.list)); + + rtw_free_recvframe(prframe, pfree_recv_queue); + } + + spin_unlock_bh(&ppending_recvframe_queue->lock); + + } + + if (!(psta->state & WIFI_AP_STATE)) + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, false); + + + /* release mac id for non-bc/mc station, */ + rtw_release_macid(pstapriv->padapter, psta); + +/* + spin_lock_bh(&pstapriv->asoc_list_lock); + list_del_init(&psta->asoc_list); + spin_unlock_bh(&pstapriv->asoc_list_lock); +*/ + spin_lock_bh(&pstapriv->auth_list_lock); + if (!list_empty(&psta->auth_list)) { + list_del_init(&psta->auth_list); + pstapriv->auth_list_cnt--; + } + spin_unlock_bh(&pstapriv->auth_list_lock); + + psta->expire_to = 0; + psta->sleepq_ac_len = 0; + psta->qos_info = 0; + + psta->max_sp_len = 0; + psta->uapsd_bk = 0; + psta->uapsd_be = 0; + psta->uapsd_vi = 0; + psta->uapsd_vo = 0; + + psta->has_legacy_ac = 0; + + pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); + pstapriv->tim_bitmap &= ~BIT(psta->aid); + + if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) { + pstapriv->sta_aid[psta->aid - 1] = NULL; + psta->aid = 0; + } + + psta->under_exist_checking = 0; + + /* spin_lock_bh(&(pfree_sta_queue->lock)); */ + list_add_tail(&psta->list, get_list_head(pfree_sta_queue)); + /* spin_unlock_bh(&(pfree_sta_queue->lock)); */ + +exit: + return _SUCCESS; +} + +/* free all stainfo which in sta_hash[all] */ +void rtw_free_all_stainfo(struct adapter *padapter) +{ + struct list_head *plist, *phead; + s32 index; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter); + + if (pstapriv->asoc_sta_count == 1) + return; + + spin_lock_bh(&pstapriv->sta_hash_lock); + + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while (phead != plist) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + plist = get_next(plist); + + if (pbcmc_stainfo != psta) + rtw_free_stainfo(padapter, psta); + + } + } + + spin_unlock_bh(&pstapriv->sta_hash_lock); +} + +/* any station allocated can be searched by hash list */ +struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr) +{ + struct list_head *plist, *phead; + struct sta_info *psta = NULL; + u32 index; + u8 *addr; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + if (hwaddr == NULL) + return NULL; + + if (IS_MCAST(hwaddr)) + addr = bc_addr; + else + addr = hwaddr; + + index = wifi_mac_hash(addr); + + spin_lock_bh(&pstapriv->sta_hash_lock); + + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + + while (phead != plist) { + + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + if ((!memcmp(psta->hwaddr, addr, ETH_ALEN))) + /* if found the matched address */ + break; + + psta = NULL; + plist = get_next(plist); + } + + spin_unlock_bh(&pstapriv->sta_hash_lock); + return psta; +} + +u32 rtw_init_bcmc_stainfo(struct adapter *padapter) +{ + + struct sta_info *psta; + struct tx_servq *ptxservq; + u32 res = _SUCCESS; + NDIS_802_11_MAC_ADDRESS bcast_addr = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + struct sta_priv *pstapriv = &padapter->stapriv; + /* struct __queue *pstapending = &padapter->xmitpriv.bm_pending; */ + + psta = rtw_alloc_stainfo(pstapriv, bcast_addr); + + if (psta == NULL) { + res = _FAIL; + RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("rtw_alloc_stainfo fail")); + goto exit; + } + + /* default broadcast & multicast use macid 1 */ + psta->mac_id = 1; + + ptxservq = &(psta->sta_xmitpriv.be_q); +exit: + return _SUCCESS; +} + + +struct sta_info *rtw_get_bcmc_stainfo(struct adapter *padapter) +{ + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + psta = rtw_get_stainfo(pstapriv, bc_addr); + return psta; +} + +u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr) +{ + u8 res = true; + struct list_head *plist, *phead; + struct rtw_wlan_acl_node *paclnode; + u8 match = false; + struct sta_priv *pstapriv = &padapter->stapriv; + struct wlan_acl_pool *pacl_list = &pstapriv->acl_list; + struct __queue *pacl_node_q = &pacl_list->acl_node_q; + + spin_lock_bh(&(pacl_node_q->lock)); + phead = get_list_head(pacl_node_q); + plist = get_next(phead); + while (phead != plist) { + paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list); + plist = get_next(plist); + + if (!memcmp(paclnode->addr, mac_addr, ETH_ALEN)) + if (paclnode->valid == true) { + match = true; + break; + } + + } + spin_unlock_bh(&(pacl_node_q->lock)); + + + if (pacl_list->mode == 1) /* accept unless in deny list */ + res = (match == true) ? false:true; + + else if (pacl_list->mode == 2)/* deny unless in accept list */ + res = (match == true) ? true:false; + else + res = true; + + return res; +} diff --git a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c new file mode 100644 index 0000000..f485f54 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c @@ -0,0 +1,2328 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> +#include <hal_com_h2c.h> + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) +#include <linux/inetdevice.h> +#endif + +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 BROADCOM_OUI3[] = {0x00, 0x05, 0xb5}; + +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 RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02}; +static unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02}; + +extern unsigned char RTW_WPA_OUI[]; +extern unsigned char WPA_TKIP_CIPHER[4]; + +#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) + +#define DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS 1000 +#define DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD 3 + +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 +}; + +int cckrates_included(unsigned char *rate, int ratelen) +{ + int i; + + for (i = 0; i < ratelen; i++) { + if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) || + (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22)) + return true; + } + + return false; + +} + +int cckratesonly_included(unsigned char *rate, int ratelen) +{ + int i; + + for (i = 0; i < ratelen; i++) { + if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) && + (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22)) + return false; + } + + return true; +} + +u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta) +{ + u8 raid, cur_rf_type, rf_type = RF_1T1R; + + rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&cur_rf_type)); + + if (cur_rf_type == RF_1T1R) { + rf_type = RF_1T1R; + } else if (IsSupportedVHT(psta->wireless_mode)) { + if (psta->ra_mask & 0xffc00000) + rf_type = RF_2T2R; + } else if (IsSupportedHT(psta->wireless_mode)) { + if (psta->ra_mask & 0xfff00000) + rf_type = RF_2T2R; + } + + switch (psta->wireless_mode) { + case WIRELESS_11B: + raid = RATEID_IDX_B; + break; + case WIRELESS_11A: + case WIRELESS_11G: + raid = RATEID_IDX_G; + break; + case WIRELESS_11BG: + raid = RATEID_IDX_BG; + break; + case WIRELESS_11_24N: + case WIRELESS_11_5N: + case WIRELESS_11A_5N: + case WIRELESS_11G_24N: + if (rf_type == RF_2T2R) + raid = RATEID_IDX_GN_N2SS; + else + raid = RATEID_IDX_GN_N1SS; + break; + case WIRELESS_11B_24N: + case WIRELESS_11BG_24N: + if (psta->bw_mode == CHANNEL_WIDTH_20) { + if (rf_type == RF_2T2R) + raid = RATEID_IDX_BGN_20M_2SS_BN; + else + raid = RATEID_IDX_BGN_20M_1SS_BN; + } else { + if (rf_type == RF_2T2R) + raid = RATEID_IDX_BGN_40M_2SS; + else + raid = RATEID_IDX_BGN_40M_1SS; + } + break; + default: + raid = RATEID_IDX_BGN_40M_2SS; + break; + + } + return raid; + +} + +unsigned char ratetbl_val_2wifirate(unsigned char rate); +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 adapter *padapter, unsigned char rate); +int is_basicrate(struct adapter *padapter, unsigned char rate) +{ + int i; + unsigned char val; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + for (i = 0; i < NumRates; i++) { + val = pmlmeext->basicrate[i]; + + if ((val != 0xff) && (val != 0xfe)) + if (rate == ratetbl_val_2wifirate(val)) + return true; + } + + return false; +} + +unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset); +unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset) +{ + int i; + unsigned char rate; + unsigned int len = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + for (i = 0; i < NumRates; i++) { + rate = pmlmeext->datarate[i]; + + switch (rate) { + case 0xff: + return len; + + case 0xfe: + continue; + + default: + rate = ratetbl_val_2wifirate(rate); + + if (is_basicrate(padapter, rate) == true) + rate |= IEEE80211_BASIC_RATE_MASK; + + rateset[len] = rate; + len++; + break; + } + } + return len; +} + +void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len) +{ + unsigned char supportedrates[NumRates]; + + memset(supportedrates, 0, NumRates); + *bssrate_len = ratetbl2rateset(padapter, supportedrates); + memcpy(pbssrate, supportedrates, *bssrate_len); +} + +void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask) +{ + u8 mcs_rate_1r = (u8)(mask&0xff); + u8 mcs_rate_2r = (u8)((mask>>8)&0xff); + u8 mcs_rate_3r = (u8)((mask>>16)&0xff); + u8 mcs_rate_4r = (u8)((mask>>24)&0xff); + + mcs_set[0] &= mcs_rate_1r; + mcs_set[1] &= mcs_rate_2r; + mcs_set[2] &= mcs_rate_3r; + mcs_set[3] &= mcs_rate_4r; +} + +void UpdateBrateTbl(struct 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; + } + } + +} + +void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen) +{ + u8 i; + u8 rate; + + for (i = 0; i < bssratelen; i++) { + rate = bssrateset[i] & 0x7f; + switch (rate) { + case IEEE80211_CCK_RATE_1MB: + case IEEE80211_CCK_RATE_2MB: + case IEEE80211_CCK_RATE_5MB: + case IEEE80211_CCK_RATE_11MB: + bssrateset[i] |= IEEE80211_BASIC_RATE_MASK; + break; + } + } + +} + +void Save_DM_Func_Flag(struct adapter *padapter) +{ + u8 bSaveFlag = true; + rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag)); +} + +void Restore_DM_Func_Flag(struct adapter *padapter) +{ + u8 bSaveFlag = false; + rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag)); +} + +void Switch_DM_Func(struct adapter *padapter, u32 mode, u8 enable) +{ + if (enable == true) + rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode)); + else + rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode)); +} + +static void Set_NETYPE0_MSR(struct adapter *padapter, u8 type) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type)); +} + +void Set_MSR(struct adapter *padapter, u8 type) +{ + Set_NETYPE0_MSR(padapter, type); +} + +inline u8 rtw_get_oper_ch(struct adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_channel; +} + +inline void rtw_set_oper_ch(struct adapter *adapter, u8 ch) +{ +#ifdef DBG_CH_SWITCH + const int len = 128; + char msg[128] = {0}; + int cnt = 0; + int i = 0; +#endif /* DBG_CH_SWITCH */ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + if (dvobj->oper_channel != ch) { + dvobj->on_oper_ch_time = jiffies; + +#ifdef DBG_CH_SWITCH + cnt += snprintf(msg+cnt, len-cnt, "switch to ch %3u", ch); + + for (i = 0; i < dvobj->iface_nums; i++) { + struct adapter *iface = dvobj->padapters[i]; + cnt += snprintf(msg+cnt, len-cnt, " ["ADPT_FMT":", ADPT_ARG(iface)); + if (iface->mlmeextpriv.cur_channel == ch) + cnt += snprintf(msg+cnt, len-cnt, "C"); + else + cnt += snprintf(msg+cnt, len-cnt, "_"); + if (iface->wdinfo.listen_channel == ch && !rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_NONE)) + cnt += snprintf(msg+cnt, len-cnt, "L"); + else + cnt += snprintf(msg+cnt, len-cnt, "_"); + cnt += snprintf(msg+cnt, len-cnt, "]"); + } + + DBG_871X(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(adapter), msg); +#endif /* DBG_CH_SWITCH */ + } + + dvobj->oper_channel = ch; +} + +inline u8 rtw_get_oper_bw(struct adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_bwmode; +} + +inline void rtw_set_oper_bw(struct adapter *adapter, u8 bw) +{ + adapter_to_dvobj(adapter)->oper_bwmode = bw; +} + +inline u8 rtw_get_oper_choffset(struct adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_ch_offset; +} + +inline void rtw_set_oper_choffset(struct adapter *adapter, u8 offset) +{ + adapter_to_dvobj(adapter)->oper_ch_offset = offset; +} + +u8 rtw_get_center_ch(u8 channel, u8 chnl_bw, u8 chnl_offset) +{ + u8 center_ch = channel; + + if (chnl_bw == CHANNEL_WIDTH_80) { + if ((channel == 36) || (channel == 40) || (channel == 44) || (channel == 48)) + center_ch = 42; + if ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64)) + center_ch = 58; + if ((channel == 100) || (channel == 104) || (channel == 108) || (channel == 112)) + center_ch = 106; + if ((channel == 116) || (channel == 120) || (channel == 124) || (channel == 128)) + center_ch = 122; + if ((channel == 132) || (channel == 136) || (channel == 140) || (channel == 144)) + center_ch = 138; + if ((channel == 149) || (channel == 153) || (channel == 157) || (channel == 161)) + center_ch = 155; + else if (channel <= 14) + center_ch = 7; + } else if (chnl_bw == CHANNEL_WIDTH_40) { + if (chnl_offset == HAL_PRIME_CHNL_OFFSET_LOWER) + center_ch = channel + 2; + else + center_ch = channel - 2; + } + + return center_ch; +} + +inline unsigned long rtw_get_on_cur_ch_time(struct adapter *adapter) +{ + if (adapter->mlmeextpriv.cur_channel == adapter_to_dvobj(adapter)->oper_channel) + return adapter_to_dvobj(adapter)->on_oper_ch_time; + else + return 0; +} + +void SelectChannel(struct adapter *padapter, unsigned char channel) +{ + if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex))) + return; + + /* saved channel info */ + rtw_set_oper_ch(padapter, channel); + + rtw_hal_set_chan(padapter, channel); + + mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex)); +} + +void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode) +{ + u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + if (padapter->bNotifyChannelChange) + DBG_871X("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode); + + center_ch = rtw_get_center_ch(channel, bwmode, channel_offset); + + if (bwmode == CHANNEL_WIDTH_80) { + if (center_ch > channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER; + else if (center_ch < channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER; + else + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + /* set Channel */ + if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex))) + return; + + /* saved channel/bw info */ + rtw_set_oper_ch(padapter, channel); + rtw_set_oper_bw(padapter, bwmode); + rtw_set_oper_choffset(padapter, channel_offset); + + rtw_hal_set_chnl_bw(padapter, center_ch, bwmode, channel_offset, chnl_offset80); /* set center channel */ + + mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex)); +} + +__inline u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork) +{ + return pnetwork->MacAddress; +} + +u16 get_beacon_interval(struct wlan_bssid_ex *bss) +{ + __le16 val; + memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); + +} + +int is_client_associated_to_ap(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + + if (!padapter) + return _FAIL; + + pmlmeext = &padapter->mlmeextpriv; + pmlmeinfo = &(pmlmeext->mlmext_info); + + if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)) + return true; + else + return _FAIL; +} + +int is_client_associated_to_ibss(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)) + return true; + else + return _FAIL; +} + +int is_IBSS_empty(struct adapter *padapter) +{ + unsigned int i; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) { + if (pmlmeinfo->FW_sta_info[i].status == 1) + return _FAIL; + } + + return true; + +} + +unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval) +{ + if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN) + return WAIT_FOR_BCN_TO_MIN; + else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX) + return WAIT_FOR_BCN_TO_MAX; + else + return ((bcn_interval << 2)); +} + +void invalidate_cam_all(struct adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, NULL); + + spin_lock_bh(&cam_ctl->lock); + cam_ctl->bitmap = 0; + memset(dvobj->cam_cache, 0, sizeof(struct cam_entry_cache)*TOTAL_CAM_ENTRY); + spin_unlock_bh(&cam_ctl->lock); +} + +static u32 _ReadCAM(struct adapter *padapter, u32 addr) +{ + u32 count = 0, cmd; + cmd = CAM_POLLINIG | addr; + rtw_write32(padapter, RWCAM, cmd); + + do { + if (0 == (rtw_read32(padapter, REG_CAMCMD) & CAM_POLLINIG)) + break; + } while (count++ < 100); + + return rtw_read32(padapter, REG_CAMREAD); +} +void read_cam(struct adapter *padapter, u8 entry, u8 *get_key) +{ + u32 j, addr, cmd; + addr = entry << 3; + + /* DBG_8192C("********* DUMP CAM Entry_#%02d***************\n", entry); */ + for (j = 0; j < 6; j++) { + cmd = _ReadCAM(padapter, addr+j); + /* DBG_8192C("offset:0x%02x => 0x%08x\n", addr+j, cmd); */ + if (j > 1) /* get key from cam */ + memcpy(get_key+(j-2)*4, &cmd, 4); + } + /* DBG_8192C("*********************************\n"); */ +} + +void _write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key) +{ + unsigned int i, val, addr; + int j; + u32 cam_val[2]; + + addr = entry << 3; + + for (j = 5; j >= 0; j--) { + switch (j) { + case 0: + val = (ctrl | (mac[0] << 16) | (mac[1] << 24)); + break; + case 1: + val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24)); + break; + default: + i = (j - 2) << 2; + val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24)); + break; + } + + cam_val[0] = val; + cam_val[1] = addr + (unsigned int)j; + + rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val); + } +} + +void _clear_cam_entry(struct adapter *padapter, u8 entry) +{ + unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + + _write_cam(padapter, entry, 0, null_sta, null_key); +} + +inline void write_cam(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key) +{ + _write_cam(adapter, id, ctrl, mac, key); + write_cam_cache(adapter, id, ctrl, mac, key); +} + +inline void clear_cam_entry(struct adapter *adapter, u8 id) +{ + _clear_cam_entry(adapter, id); + clear_cam_cache(adapter, id); +} + +inline void write_cam_from_cache(struct adapter *adapter, u8 id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + struct cam_entry_cache cache; + + spin_lock_bh(&cam_ctl->lock); + memcpy(&cache, &dvobj->cam_cache[id], sizeof(struct cam_entry_cache)); + spin_unlock_bh(&cam_ctl->lock); + + _write_cam(adapter, id, cache.ctrl, cache.mac, cache.key); +} + +void write_cam_cache(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + spin_lock_bh(&cam_ctl->lock); + + dvobj->cam_cache[id].ctrl = ctrl; + memcpy(dvobj->cam_cache[id].mac, mac, ETH_ALEN); + memcpy(dvobj->cam_cache[id].key, key, 16); + + spin_unlock_bh(&cam_ctl->lock); +} + +void clear_cam_cache(struct adapter *adapter, u8 id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + spin_lock_bh(&cam_ctl->lock); + + memset(&(dvobj->cam_cache[id]), 0, sizeof(struct cam_entry_cache)); + + spin_unlock_bh(&cam_ctl->lock); +} + +static bool _rtw_camid_is_gk(struct adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + bool ret = false; + + if (cam_id >= TOTAL_CAM_ENTRY) + goto exit; + + if (!(cam_ctl->bitmap & BIT(cam_id))) + goto exit; + + ret = (dvobj->cam_cache[cam_id].ctrl&BIT6)?true:false; + +exit: + return ret; +} + +static s16 _rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + int i; + s16 cam_id = -1; + + for (i = 0; i < TOTAL_CAM_ENTRY; i++) { + if (addr && memcmp(dvobj->cam_cache[i].mac, addr, ETH_ALEN)) + continue; + if (kid >= 0 && kid != (dvobj->cam_cache[i].ctrl&0x03)) + continue; + + cam_id = i; + break; + } + + if (addr) + DBG_871X(FUNC_ADPT_FMT" addr:"MAC_FMT" kid:%d, return cam_id:%d\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid, cam_id); + else + DBG_871X(FUNC_ADPT_FMT" addr:%p kid:%d, return cam_id:%d\n" + , FUNC_ADPT_ARG(adapter), addr, kid, cam_id); + + return cam_id; +} + +s16 rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + s16 cam_id = -1; + + spin_lock_bh(&cam_ctl->lock); + cam_id = _rtw_camid_search(adapter, addr, kid); + spin_unlock_bh(&cam_ctl->lock); + + return cam_id; +} + +s16 rtw_camid_alloc(struct adapter *adapter, struct sta_info *sta, u8 kid) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + s16 cam_id = -1; + struct mlme_ext_info *mlmeinfo; + + spin_lock_bh(&cam_ctl->lock); + + mlmeinfo = &adapter->mlmeextpriv.mlmext_info; + + if ((((mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) || ((mlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)) + && !sta) { + /* AP/Ad-hoc mode group key: static alloction to default key by key ID */ + if (kid > 3) { + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" group key with invalid key id:%u\n" + , FUNC_ADPT_ARG(adapter), kid); + rtw_warn_on(1); + goto bitmap_handle; + } + + cam_id = kid; + } else { + int i; + u8 *addr = sta?sta->hwaddr:NULL; + + if (!sta) { + if (!(mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + /* bypass STA mode group key setting before connected(ex:WEP) because bssid is not ready */ + goto bitmap_handle; + } + + addr = get_bssid(&adapter->mlmepriv); + } + + i = _rtw_camid_search(adapter, addr, kid); + if (i >= 0) { + /* Fix issue that pairwise and group key have same key id. Pairwise key first, group key can overwirte group only(ex: rekey) */ + if (sta || _rtw_camid_is_gk(adapter, i) == true) + cam_id = i; + else + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" group key id:%u the same key id as pairwise key\n" + , FUNC_ADPT_ARG(adapter), kid); + goto bitmap_handle; + } + + for (i = 4; i < TOTAL_CAM_ENTRY; i++) + if (!(cam_ctl->bitmap & BIT(i))) + break; + + if (i == TOTAL_CAM_ENTRY) { + if (sta) + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u no room\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->hwaddr), kid); + else + DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" group key id:%u no room\n" + , FUNC_ADPT_ARG(adapter), kid); + rtw_warn_on(1); + goto bitmap_handle; + } + + cam_id = i; + } + +bitmap_handle: + if (cam_id >= 0 && cam_id < 32) + cam_ctl->bitmap |= BIT(cam_id); + + spin_unlock_bh(&cam_ctl->lock); + + return cam_id; +} + +void rtw_camid_free(struct adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + spin_lock_bh(&cam_ctl->lock); + + if (cam_id < TOTAL_CAM_ENTRY) + cam_ctl->bitmap &= ~(BIT(cam_id)); + + spin_unlock_bh(&cam_ctl->lock); +} + +int allocate_fw_sta_entry(struct adapter *padapter) +{ + unsigned int mac_id; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) { + if (pmlmeinfo->FW_sta_info[mac_id].status == 0) { + pmlmeinfo->FW_sta_info[mac_id].status = 1; + pmlmeinfo->FW_sta_info[mac_id].retry = 0; + break; + } + } + + return mac_id; +} + +void flush_all_cam_entry(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + invalidate_cam_all(padapter); + /* clear default key related key search setting */ + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)false); + + memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info)); + +} + +int WMM_param_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE) +{ + /* struct registry_priv *pregpriv = &padapter->registrypriv; */ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pmlmepriv->qospriv.qos_option == 0) { + pmlmeinfo->WMM_enable = 0; + return false; + } + + if (!memcmp(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element))) + return false; + else + memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element)); + + pmlmeinfo->WMM_enable = 1; + return true; +} + +void WMMOnAssocRsp(struct adapter *padapter) +{ + u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime; + u8 acm_mask; + u16 TXOP; + u32 acParm, i; + u32 edca[4], inx[4]; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + + acm_mask = 0; + + if (pmlmeext->cur_wireless_mode & WIRELESS_11_24N) + aSifsTime = 16; + else + aSifsTime = 10; + + if (pmlmeinfo->WMM_enable == 0) { + padapter->mlmepriv.acm_mask = 0; + + AIFS = aSifsTime + (2 * pmlmeinfo->slotTime); + + if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11A)) { + ECWMin = 4; + ECWMax = 10; + } else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) { + ECWMin = 5; + ECWMax = 10; + } else { + ECWMin = 4; + ECWMax = 10; + } + + TXOP = 0; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + + ECWMin = 2; + ECWMax = 3; + TXOP = 0x2f; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + } else{ + edca[0] = edca[1] = edca[2] = edca[3] = 0; + + for (i = 0; i < 4; i++) { + ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03; + ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01; + + /* AIFS = AIFSN * slot time + SIFS - r2t phy delay */ + AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime; + + ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f); + ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4; + TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit); + + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + + switch (ACI) { + case 0x0: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(1):0); + edca[XMIT_BE_QUEUE] = acParm; + break; + + case 0x1: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + /* acm_mask |= (ACM? BIT(0):0); */ + edca[XMIT_BK_QUEUE] = acParm; + break; + + case 0x2: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(2):0); + edca[XMIT_VI_QUEUE] = acParm; + break; + + case 0x3: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(3):0); + edca[XMIT_VO_QUEUE] = acParm; + break; + } + + DBG_871X("WMM(%x): %x, %x\n", ACI, ACM, acParm); + } + + if (padapter->registrypriv.acm_method == 1) + rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask)); + else + padapter->mlmepriv.acm_mask = acm_mask; + + inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3; + + if (pregpriv->wifi_spec == 1) { + u32 j, tmp, change_inx = false; + + /* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */ + for (i = 0; i < 4; i++) { + for (j = i+1; j < 4; j++) { + /* compare CW and AIFS */ + if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) { + change_inx = true; + } else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) { + /* compare TXOP */ + if ((edca[j] >> 16) > (edca[i] >> 16)) + change_inx = true; + } + + if (change_inx) { + tmp = edca[i]; + edca[i] = edca[j]; + edca[j] = tmp; + + tmp = inx[i]; + inx[i] = inx[j]; + inx[j] = tmp; + + change_inx = false; + } + } + } + } + + for (i = 0; i < 4; i++) { + pxmitpriv->wmm_para_seq[i] = inx[i]; + DBG_871X("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]); + } + } +} + +static void bwmode_update_check(struct adapter *padapter, struct ndis_80211_var_ie *pIE) +{ + unsigned char new_bwmode; + unsigned char new_ch_offset; + struct HT_info_element *pHT_info; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + u8 cbw40_enable = 0; + + if (!pIE) + return; + + if (phtpriv->ht_option == false) + return; + + if (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_80) + return; + + if (pIE->Length > sizeof(struct HT_info_element)) + return; + + pHT_info = (struct HT_info_element *)pIE->data; + + if (pmlmeext->cur_channel > 14) { + if ((pregistrypriv->bw_mode & 0xf0) > 0) + cbw40_enable = 1; + } else + if ((pregistrypriv->bw_mode & 0x0f) > 0) + cbw40_enable = 1; + + if ((pHT_info->infos[0] & BIT(2)) && cbw40_enable) { + new_bwmode = 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_bwmode = CHANNEL_WIDTH_20; + new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } else{ + new_bwmode = CHANNEL_WIDTH_20; + new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + + if ((new_bwmode != pmlmeext->cur_bwmode) || (new_ch_offset != pmlmeext->cur_ch_offset)) { + pmlmeinfo->bwmode_updated = true; + + pmlmeext->cur_bwmode = new_bwmode; + pmlmeext->cur_ch_offset = new_ch_offset; + + /* update HT info also */ + HT_info_handler(padapter, pIE); + } else + pmlmeinfo->bwmode_updated = false; + + + if (true == pmlmeinfo->bwmode_updated) { + struct sta_info *psta; + struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); + struct sta_priv *pstapriv = &padapter->stapriv; + + /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + + + /* update ap's stainfo */ + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (psta) { + struct ht_priv *phtpriv_sta = &psta->htpriv; + + if (phtpriv_sta->ht_option) { + /* bwmode */ + psta->bw_mode = pmlmeext->cur_bwmode; + phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset; + } else{ + psta->bw_mode = CHANNEL_WIDTH_20; + phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta); + } + } +} + +void HT_caps_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE) +{ + unsigned int i; + u8 rf_type; + u8 max_AMPDU_len, min_MPDU_spacing; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0; + 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; + } + } + rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type)); + + /* update the MCS set */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i]; + + /* update the MCS rates */ + switch (rf_type) { + case RF_1T1R: + case RF_1T2R: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R); + break; + case RF_2T2R: + default: +#ifdef CONFIG_DISABLE_MCS13TO15 + if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1) + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF); + else + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); +#else /* CONFIG_DISABLE_MCS13TO15 */ + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); +#endif /* CONFIG_DISABLE_MCS13TO15 */ + } + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + /* Config STBC setting */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAPABILITY_ELE_TX_STBC(pIE->data)) { + SET_FLAG(cur_stbc_cap, STBC_HT_ENABLE_TX); + DBG_871X("Enable HT Tx STBC !\n"); + } + phtpriv->stbc_cap = cur_stbc_cap; + } else { + /* Config LDPC Coding Capability */ + if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAPABILITY_ELE_LDPC_CAP(pIE->data)) { + SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX)); + DBG_871X("Enable HT Tx LDPC!\n"); + } + phtpriv->ldpc_cap = cur_ldpc_cap; + + /* Config STBC setting */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAPABILITY_ELE_RX_STBC(pIE->data)) { + SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX)); + DBG_871X("Enable HT Tx STBC!\n"); + } + phtpriv->stbc_cap = cur_stbc_cap; + } +} + +void HT_info_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + + if (pIE == NULL) + return; + + if (phtpriv->ht_option == 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 HTOnAssocRsp(struct adapter *padapter) +{ + unsigned char max_AMPDU_len; + unsigned char min_MPDU_spacing; + /* struct registry_priv *pregpriv = &padapter->registrypriv; */ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + DBG_871X("%s\n", __func__); + + if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) { + pmlmeinfo->HT_enable = 1; + } else{ + pmlmeinfo->HT_enable = 0; + /* set_channel_bwmode(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_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing)); + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); +} + +void ERP_IE_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE->Length > 1) + return; + + pmlmeinfo->ERP_enable = 1; + memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->Length); +} + +void VCS_update(struct adapter *padapter, struct sta_info *psta) +{ + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + switch (pregpriv->vrtl_carrier_sense) {/* 0:off 1:on 2:auto */ + case 0: /* off */ + psta->rtsen = 0; + psta->cts2self = 0; + break; + + case 1: /* on */ + if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */ + psta->rtsen = 1; + psta->cts2self = 0; + } else{ + psta->rtsen = 0; + psta->cts2self = 1; + } + break; + + case 2: /* auto */ + default: + if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) { + if (pregpriv->vcs_type == 1) { + psta->rtsen = 1; + psta->cts2self = 0; + } else{ + psta->rtsen = 0; + psta->cts2self = 1; + } + } else{ + psta->rtsen = 0; + psta->cts2self = 0; + } + break; + } +} + +void update_ldpc_stbc_cap(struct sta_info *psta) +{ + if (psta->htpriv.ht_option) { + if (TEST_FLAG(psta->htpriv.ldpc_cap, LDPC_HT_ENABLE_TX)) + psta->ldpc = 1; + + if (TEST_FLAG(psta->htpriv.stbc_cap, STBC_HT_ENABLE_TX)) + psta->stbc = 1; + } else { + psta->ldpc = 0; + psta->stbc = 0; + } +} + +int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len) +{ + unsigned int len; + unsigned char *p; + unsigned short val16, subtype; + struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network); + /* u8 wpa_ie[255], rsn_ie[255]; */ + u16 wpa_len = 0, rsn_len = 0; + u8 encryp_protocol = 0; + struct wlan_bssid_ex *bssid; + int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0; + unsigned char *pbuf; + u32 wpa_ielen = 0; + u8 *pbssid = GetAddr3Ptr(pframe); + u32 hidden_ssid = 0; + struct HT_info_element *pht_info = NULL; + struct rtw_ieee80211_ht_cap *pht_cap = NULL; + u32 bcn_channel; + unsigned short ht_cap_info; + unsigned char ht_info_infos_0; + struct mlme_priv *pmlmepriv = &Adapter->mlmepriv; + + if (is_client_associated_to_ap(Adapter) == false) + return true; + + len = packet_len - sizeof(struct ieee80211_hdr_3addr); + + if (len > MAX_IE_SZ) { + DBG_871X("%s IE too long for survey event\n", __func__); + return _FAIL; + } + + if (memcmp(cur_network->network.MacAddress, pbssid, 6)) { + DBG_871X("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 *)rtw_zmalloc(sizeof(struct wlan_bssid_ex)); + if (bssid == NULL) { + DBG_871X("%s rtw_zmalloc fail !!!\n", __func__); + return true; + } + + if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) > DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS)) { + pmlmepriv->timeBcnInfoChkStart = 0; + pmlmepriv->NumOfBcnInfoChkFail = 0; + } + + subtype = GetFrameSubType(pframe) >> 4; + + if (subtype == WIFI_BEACON) + bssid->Reserved[0] = 1; + + bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len; + + /* below is to copy the information element */ + bssid->IELength = len; + memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength); + + /* check bw and channel offset */ + /* parsing HT_CAP_IE */ + p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (p && len > 0) { + pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2); + ht_cap_info = le16_to_cpu(pht_cap->cap_info); + } else { + ht_cap_info = 0; + } + /* parsing HT_INFO_IE */ + p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (p && len > 0) { + pht_info = (struct HT_info_element *)(p + 2); + ht_info_infos_0 = pht_info->infos[0]; + } else { + ht_info_infos_0 = 0; + } + if (ht_cap_info != cur_network->BcnInfo.ht_cap_info || + ((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) { + DBG_871X("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__, + ht_cap_info, ht_info_infos_0); + DBG_871X("%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_871X("%s bw mode change\n", __func__); + { + /* bcn_info_update */ + cur_network->BcnInfo.ht_cap_info = ht_cap_info; + cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0; + /* to do : need to check that whether modify related register of BB or not */ + } + /* goto _mismatch; */ + } + + /* Checking for channel */ + p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (p) { + bcn_channel = *(p + 2); + } else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */ + rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (pht_info) { + bcn_channel = pht_info->primary_channel; + } else { /* we don't find channel IE, so don't check it */ + /* DBG_871X("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_871X("%s beacon channel:%d cur channel:%d disconnect\n", __func__, + bcn_channel, Adapter->mlmeextpriv.cur_channel); + goto _mismatch; + } + + /* checking SSID */ + p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (p == NULL) { + DBG_871X("%s marc: cannot find SSID for survey event\n", __func__); + hidden_ssid = true; + } else { + hidden_ssid = false; + } + + if ((NULL != p) && (false == hidden_ssid && (*(p + 1)))) { + memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1)); + bssid->Ssid.SsidLength = *(p + 1); + } else { + bssid->Ssid.SsidLength = 0; + bssid->Ssid.Ssid[0] = '\0'; + } + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d " + "cur_network->network.Ssid.Ssid:%s len:%d\n", __func__, bssid->Ssid.Ssid, + bssid->Ssid.SsidLength, cur_network->network.Ssid.Ssid, + cur_network->network.Ssid.SsidLength)); + + if (memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) || + bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) { + if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0) { /* not hidden ssid */ + DBG_871X("%s(), SSID is not match\n", __func__); + goto _mismatch; + } + } + + /* check encryption info */ + val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid); + + if (val16 & BIT(4)) + bssid->Privacy = 1; + else + bssid->Privacy = 0; + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n", + __func__, cur_network->network.Privacy, bssid->Privacy)); + if (cur_network->network.Privacy != bssid->Privacy) { + DBG_871X("%s(), privacy is not match\n", __func__); + goto _mismatch; + } + + rtw_get_sec_ie(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL, &wpa_len); + + if (rsn_len > 0) { + encryp_protocol = ENCRYP_PROTOCOL_WPA2; + } else if (wpa_len > 0) { + encryp_protocol = ENCRYP_PROTOCOL_WPA; + } else { + if (bssid->Privacy) + encryp_protocol = ENCRYP_PROTOCOL_WEP; + } + + if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) { + DBG_871X("%s(): enctyp is not match\n", __func__); + goto _mismatch; + } + + if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) { + pbuf = rtw_get_wpa_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12); + if (pbuf && (wpa_ielen > 0)) { + if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__, + pairwise_cipher, group_cipher, is_8021x)); + } + } else { + pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12); + + if (pbuf && (wpa_ielen > 0)) { + if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) { + RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, + ("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n", + __func__, pairwise_cipher, group_cipher, is_8021x)); + } + } + } + + RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, + ("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher)); + if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) { + DBG_871X("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match\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_871X("%s authentication is not match\n", __func__); + goto _mismatch; + } + } + + kfree((u8 *)bssid); + return _SUCCESS; + +_mismatch: + kfree((u8 *)bssid); + + if (pmlmepriv->NumOfBcnInfoChkFail == 0) + pmlmepriv->timeBcnInfoChkStart = jiffies; + + pmlmepriv->NumOfBcnInfoChkFail++; + DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d (SeqNum of this Beacon frame = %d).\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail, GetSequence(pframe)); + + if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) <= DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS) + && (pmlmepriv->NumOfBcnInfoChkFail >= DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD)) { + DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d >= threshold : %d (in %d ms), return FAIL.\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail, + DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD, jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart)); + pmlmepriv->timeBcnInfoChkStart = 0; + pmlmepriv->NumOfBcnInfoChkFail = 0; + return _FAIL; + } + + return _SUCCESS; +} + +void update_beacon_info(struct adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta) +{ + unsigned int i; + unsigned int len; + struct ndis_80211_var_ie *pIE; + + len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN); + + for (i = 0; i < len;) { + pIE = (struct ndis_80211_var_ie *)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + /* to update WMM paramter set while receiving beacon */ + if (!memcmp(pIE->data, WMM_PARA_OUI, 6) && pIE->Length == WLAN_WMM_LEN) /* WMM */ + if (WMM_param_handler(padapter, pIE)) + report_wmm_edca_update(padapter); + + break; + + case _HT_EXTRA_INFO_IE_: /* HT info */ + /* HT_info_handler(padapter, pIE); */ + bwmode_update_check(padapter, pIE); + break; + + case _ERPINFO_IE_: + ERP_IE_handler(padapter, pIE); + VCS_update(padapter, psta); + break; + + default: + break; + } + + i += (pIE->Length + 2); + } +} + +unsigned int is_ap_in_tkip(struct adapter *padapter) +{ + u32 i; + struct ndis_80211_var_ie *pIE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); + + if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.IELength;) { + pIE = (struct ndis_80211_var_ie *)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4))) + return true; + + break; + + case _RSN_IE_2_: + if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4)) + return true; + + default: + break; + } + + i += (pIE->Length + 2); + } + + return false; + } else + return false; + +} + +int support_short_GI(struct adapter *padapter, struct HT_caps_element *pHT_caps, u8 bwmode) +{ + unsigned char bit_offset; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (!(pmlmeinfo->HT_enable)) + return _FAIL; + + bit_offset = (bwmode & CHANNEL_WIDTH_40) ? 6 : 5; + + if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & (0x1 << bit_offset)) + return _SUCCESS; + else + return _FAIL; +} + +unsigned char get_highest_rate_idx(u32 mask) +{ + int i; + unsigned char rate_idx = 0; + + for (i = 31; i >= 0; i--) { + if (mask & BIT(i)) { + rate_idx = i; + break; + } + } + + return rate_idx; +} + +void Update_RA_Entry(struct adapter *padapter, struct sta_info *psta) +{ + rtw_hal_update_ra_mask(psta, 0); +} + +void enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta); +void enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta) +{ + Update_RA_Entry(padapter, psta); +} + +void set_sta_rate(struct adapter *padapter, struct sta_info *psta) +{ + /* rate adaptive */ + enable_rate_adaptive(padapter, psta); +} + +unsigned char check_assoc_AP(u8 *pframe, uint len) +{ + unsigned int i; + struct ndis_80211_var_ie *pIE; + + for (i = sizeof(struct ndis_802_11_fix_ie); i < len;) { + pIE = (struct ndis_80211_var_ie *)(pframe + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) { + DBG_871X("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_OUI3, 3))) { + DBG_871X("link to Broadcom AP\n"); + return HT_IOT_PEER_BROADCOM; + } else if (!memcmp(pIE->data, MARVELL_OUI, 3)) { + DBG_871X("link to Marvell AP\n"); + return HT_IOT_PEER_MARVELL; + } else if (!memcmp(pIE->data, RALINK_OUI, 3)) { + DBG_871X("link to Ralink AP\n"); + return HT_IOT_PEER_RALINK; + } else if (!memcmp(pIE->data, CISCO_OUI, 3)) { + DBG_871X("link to Cisco AP\n"); + return HT_IOT_PEER_CISCO; + } else if (!memcmp(pIE->data, REALTEK_OUI, 3)) { + u32 Vender = HT_IOT_PEER_REALTEK; + + if (pIE->Length >= 5) { + if (pIE->data[4] == 1) + /* if (pIE->data[5] & RT_HT_CAP_USE_LONG_PREAMBLE) */ + /* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_LONG_PREAMBLE; */ + if (pIE->data[5] & RT_HT_CAP_USE_92SE) + /* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE; */ + Vender = HT_IOT_PEER_REALTEK_92SE; + + if (pIE->data[5] & RT_HT_CAP_USE_SOFTAP) + Vender = HT_IOT_PEER_REALTEK_SOFTAP; + + if (pIE->data[4] == 2) { + if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_BCUT) { + Vender = HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP; + DBG_871X("link to Realtek JAGUAR_BCUTAP\n"); + } + if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT) { + Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP; + DBG_871X("link to Realtek JAGUAR_CCUTAP\n"); + } + } + } + + DBG_871X("link to Realtek AP\n"); + return Vender; + } else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) { + DBG_871X("link to Airgo Cap\n"); + return HT_IOT_PEER_AIRGO; + } else + break; + + default: + break; + } + + i += (pIE->Length + 2); + } + + DBG_871X("link to new AP\n"); + return HT_IOT_PEER_UNKNOWN; +} + +void update_IOT_info(struct adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + switch (pmlmeinfo->assoc_AP_vendor) { + case HT_IOT_PEER_MARVELL: + pmlmeinfo->turboMode_cts2self = 1; + pmlmeinfo->turboMode_rtsen = 0; + break; + + case HT_IOT_PEER_RALINK: + pmlmeinfo->turboMode_cts2self = 0; + pmlmeinfo->turboMode_rtsen = 1; + /* disable high power */ + Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false); + break; + case HT_IOT_PEER_REALTEK: + /* rtw_write16(padapter, 0x4cc, 0xffff); */ + /* rtw_write16(padapter, 0x546, 0x01c0); */ + /* disable high power */ + Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false); + break; + default: + pmlmeinfo->turboMode_cts2self = 0; + pmlmeinfo->turboMode_rtsen = 1; + break; + } + +} + +void update_capinfo(struct adapter *Adapter, u16 updateCap) +{ + struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + bool ShortPreamble; + + /* Check preamble mode, 2005.01.06, by rcnjko. */ + /* Mark to update preamble value forever, 2008.03.18 by lanhsin */ + /* if (pMgntInfo->RegPreambleMode == PREAMBLE_AUTO) */ + { + + if (updateCap & cShortPreamble) { + /* Short Preamble */ + if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /* PREAMBLE_LONG or PREAMBLE_AUTO */ + ShortPreamble = true; + pmlmeinfo->preamble_mode = PREAMBLE_SHORT; + rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble); + } + } else{ + /* Long Preamble */ + if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) { /* PREAMBLE_SHORT or PREAMBLE_AUTO */ + ShortPreamble = false; + pmlmeinfo->preamble_mode = PREAMBLE_LONG; + rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble); + } + } + } + + if (updateCap & cIBSS) + /* Filen: See 802.11-2007 p.91 */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + else { + /* Filen: See 802.11-2007 p.90 */ + if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N | WIRELESS_11A | WIRELESS_11_5N | WIRELESS_11AC)) + pmlmeinfo->slotTime = SHORT_SLOT_TIME; + else if (pmlmeext->cur_wireless_mode & (WIRELESS_11G)) { + if ((updateCap & cShortSlotTime) /* && (!(pMgntInfo->pHTInfo->RT2RT_HT_Mode & RT_HT_CAP_USE_LONG_PREAMBLE)) */) + /* Short Slot Time */ + pmlmeinfo->slotTime = SHORT_SLOT_TIME; + else + /* Long Slot Time */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + } else + /* B Mode */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + } + + rtw_hal_set_hwreg(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime); + +} + +void update_wireless_mode(struct adapter *padapter) +{ + int ratelen, network_type = 0; + u32 SIFS_Timer; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); + unsigned char *rate = cur_network->SupportedRates; + + ratelen = rtw_get_rateset_len(cur_network->SupportedRates); + + if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) + pmlmeinfo->HT_enable = 1; + + if (pmlmeext->cur_channel > 14) { + if (pmlmeinfo->VHT_enable) + network_type = WIRELESS_11AC; + else if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_5N; + + network_type |= WIRELESS_11A; + } else{ + if (pmlmeinfo->VHT_enable) + network_type = WIRELESS_11AC; + else if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_24N; + + if ((cckratesonly_included(rate, ratelen)) == true) + network_type |= WIRELESS_11B; + else if ((cckrates_included(rate, ratelen)) == true) + network_type |= WIRELESS_11BG; + else + network_type |= WIRELESS_11G; + } + + pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode; + + SIFS_Timer = 0x0a0a0808; /* 0x0808 -> for CCK, 0x0a0a -> for OFDM */ + /* change this value if having IOT issues. */ + + padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS, (u8 *)&SIFS_Timer); + + padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WIRELESS_MODE, (u8 *)&(pmlmeext->cur_wireless_mode)); + + if (pmlmeext->cur_wireless_mode & WIRELESS_11B) + update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB); + else + update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB); +} + +void update_sta_basic_rate(struct sta_info *psta, u8 wireless_mode) +{ + if (IsSupportedTxCCK(wireless_mode)) { + /* Only B, B/G, and B/G/N AP could use CCK rate */ + memcpy(psta->bssrateset, rtw_basic_rate_cck, 4); + psta->bssratelen = 4; + } else{ + memcpy(psta->bssrateset, rtw_basic_rate_ofdm, 3); + psta->bssratelen = 3; + } +} + +int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx) +{ + unsigned int ie_len; + struct ndis_80211_var_ie *pIE; + int supportRateNum = 0; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len); + if (pIE == NULL) + return _FAIL; + + memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len); + supportRateNum = ie_len; + + pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len); + if (pIE) + memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len); + + return _SUCCESS; + +} + +void process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr) +{ + struct sta_info *psta; + u16 tid, 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_stainfo(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]; + + #ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ + preorder_ctrl->indicate_seq = start_seq; + #ifdef DBG_RX_SEQ + DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, start_seq: %d\n", __func__, __LINE__, + preorder_ctrl->indicate_seq, start_seq); + #endif + #else + preorder_ctrl->indicate_seq = 0xffff; + #endif + + preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true) ? true : false; + } + +} + +void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len) +{ + u8 *pIE; + __le32 *pbuf; + + pIE = pframe + sizeof(struct ieee80211_hdr_3addr); + pbuf = (__le32 *)pIE; + + pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1)); + + pmlmeext->TSFValue = pmlmeext->TSFValue << 32; + + pmlmeext->TSFValue |= le32_to_cpu(*pbuf); +} + +void correct_TSF(struct adapter *padapter, struct mlme_ext_priv *pmlmeext) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, NULL); +} + +void adaptive_early_32k(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len) +{ + int i; + u8 *pIE; + __le32 *pbuf; + u64 tsf = 0; + u32 delay_ms; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + + pmlmeext->bcn_cnt++; + + pIE = pframe + sizeof(struct ieee80211_hdr_3addr); + pbuf = (__le32 *)pIE; + + tsf = le32_to_cpu(*(pbuf+1)); + tsf = tsf << 32; + tsf |= le32_to_cpu(*pbuf); + + /* DBG_871X("%s(): tsf_upper = 0x%08x, tsf_lower = 0x%08x\n", __func__, (u32)(tsf>>32), (u32)tsf); */ + + /* delay = (timestamp mod 1024*100)/1000 (unit: ms) */ + /* delay_ms = do_div(tsf, (pmlmeinfo->bcn_interval*1024))/1000; */ + delay_ms = rtw_modular64(tsf, (pmlmeinfo->bcn_interval*1024)); + delay_ms = delay_ms/1000; + + if (delay_ms >= 8) + pmlmeext->bcn_delay_cnt[8]++; + /* pmlmeext->bcn_delay_ratio[8] = (pmlmeext->bcn_delay_cnt[8] * 100) /pmlmeext->bcn_cnt; */ + else + pmlmeext->bcn_delay_cnt[delay_ms]++; + /* pmlmeext->bcn_delay_ratio[delay_ms] = (pmlmeext->bcn_delay_cnt[delay_ms] * 100) /pmlmeext->bcn_cnt; */ + +/* + DBG_871X("%s(): (a)bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt); + + + for (i = 0; i<9; i++) + { + DBG_871X("%s():bcn_delay_cnt[%d]=%d, bcn_delay_ratio[%d]=%d\n", __func__, i, + pmlmeext->bcn_delay_cnt[i] , i, pmlmeext->bcn_delay_ratio[i]); + } +*/ + + /* dump for adaptive_early_32k */ + if (pmlmeext->bcn_cnt > 100 && (pmlmeext->adaptive_tsf_done == true)) { + u8 ratio_20_delay, ratio_80_delay; + u8 DrvBcnEarly, DrvBcnTimeOut; + + ratio_20_delay = 0; + ratio_80_delay = 0; + DrvBcnEarly = 0xff; + DrvBcnTimeOut = 0xff; + + DBG_871X("%s(): bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt); + + for (i = 0; i < 9; i++) { + pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i] * 100) / pmlmeext->bcn_cnt; + + + DBG_871X("%s():bcn_delay_cnt[%d]=%d, bcn_delay_ratio[%d]=%d\n", __func__, i, + pmlmeext->bcn_delay_cnt[i], i, pmlmeext->bcn_delay_ratio[i]); + + ratio_20_delay += pmlmeext->bcn_delay_ratio[i]; + ratio_80_delay += pmlmeext->bcn_delay_ratio[i]; + + if (ratio_20_delay > 20 && DrvBcnEarly == 0xff) { + DrvBcnEarly = i; + DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, DrvBcnEarly); + } + + if (ratio_80_delay > 80 && DrvBcnTimeOut == 0xff) { + DrvBcnTimeOut = i; + DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, DrvBcnTimeOut); + } + + /* reset adaptive_early_32k cnt */ + pmlmeext->bcn_delay_cnt[i] = 0; + pmlmeext->bcn_delay_ratio[i] = 0; + } + + pmlmeext->DrvBcnEarly = DrvBcnEarly; + pmlmeext->DrvBcnTimeOut = DrvBcnTimeOut; + + pmlmeext->bcn_cnt = 0; + } + +} + + +void beacon_timing_control(struct adapter *padapter) +{ + rtw_hal_bcn_related_reg_setting(padapter); +} + +void rtw_alloc_macid(struct adapter *padapter, struct sta_info *psta) +{ + int i; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + + + if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN)) + return; + + if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN)) { + psta->mac_id = NUM_STA; + return; + } + + spin_lock_bh(&pdvobj->lock); + for (i = 0; i < NUM_STA; i++) { + if (pdvobj->macid[i] == false) { + pdvobj->macid[i] = true; + break; + } + } + spin_unlock_bh(&pdvobj->lock); + + if (i > (NUM_STA-1)) { + psta->mac_id = NUM_STA; + DBG_871X(" no room for more MACIDs\n"); + } else{ + psta->mac_id = i; + DBG_871X("%s = %d\n", __func__, psta->mac_id); + } + +} + +void rtw_release_macid(struct adapter *padapter, struct sta_info *psta) +{ + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + + + if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN)) + return; + + if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN)) + return; + + spin_lock_bh(&pdvobj->lock); + if (psta->mac_id < NUM_STA && psta->mac_id != 1) { + if (pdvobj->macid[psta->mac_id] == true) { + DBG_871X("%s = %d\n", __func__, psta->mac_id); + pdvobj->macid[psta->mac_id] = false; + psta->mac_id = NUM_STA; + } + + } + spin_unlock_bh(&pdvobj->lock); + +} +/* For 8188E RA */ +u8 rtw_search_max_mac_id(struct adapter *padapter) +{ + u8 max_mac_id = 0; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + int i; + spin_lock_bh(&pdvobj->lock); + for (i = (NUM_STA-1); i >= 0 ; i--) { + if (pdvobj->macid[i] == true) + break; + } + max_mac_id = i; + spin_unlock_bh(&pdvobj->lock); + + return max_mac_id; + +} + +struct adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj) +{ + if (get_iface_type(dvobj->padapters[i]) != IFACE_PORT0) + return NULL; + + return dvobj->padapters; +} + +#ifdef CONFIG_GPIO_API +int rtw_get_gpio(struct net_device *netdev, int gpio_num) +{ + u8 value; + u8 direction; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + + rtw_ps_deny(adapter, PS_DENY_IOCTL); + + DBG_871X("rf_pwrstate = 0x%02x\n", pwrpriv->rf_pwrstate); + LeaveAllPowerSaveModeDirect(adapter); + + /* Read GPIO Direction */ + direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num; + + /* According the direction to read register value */ + if (direction) + value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & BIT(gpio_num)) >> gpio_num; + else + value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL) & BIT(gpio_num)) >> gpio_num; + + rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL); + DBG_871X("%s direction =%d value =%d\n", __func__, direction, value); + + return value; +} +EXPORT_SYMBOL(rtw_get_gpio); + +int rtw_set_gpio_output_value(struct net_device *netdev, int gpio_num, bool isHigh) +{ + u8 direction = 0; + u8 res = -1; + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev); + + /* Check GPIO is 4~7 */ + if (gpio_num > 7 || gpio_num < 4) { + DBG_871X("%s The gpio number does not included 4~7.\n", __func__); + return -1; + } + + rtw_ps_deny(adapter, PS_DENY_IOCTL); + + LeaveAllPowerSaveModeDirect(adapter); + + /* Read GPIO direction */ + direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num; + + /* If GPIO is output direction, setting value. */ + if (direction) { + if (isHigh) + rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) | BIT(gpio_num)); + else + rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & ~BIT(gpio_num)); + + DBG_871X("%s Set gpio %x[%d]=%d\n", __func__, REG_GPIO_PIN_CTRL+1, gpio_num, isHigh); + res = 0; + } else{ + DBG_871X("%s The gpio is input, not be set!\n", __func__); + res = -1; + } + + rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL); + return res; +} +EXPORT_SYMBOL(rtw_set_gpio_output_value); + +int rtw_config_gpio(struct net_device *netdev, int gpio_num, bool isOutput) +{ + struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev); + + if (gpio_num > 7 || gpio_num < 4) { + DBG_871X("%s The gpio number does not included 4~7.\n", __func__); + return -1; + } + + DBG_871X("%s gpio_num =%d direction =%d\n", __func__, gpio_num, isOutput); + + rtw_ps_deny(adapter, PS_DENY_IOCTL); + + LeaveAllPowerSaveModeDirect(adapter); + + if (isOutput) + rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) | BIT(gpio_num)); + else + rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & ~BIT(gpio_num)); + + rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL); + + return 0; +} +EXPORT_SYMBOL(rtw_config_gpio); +#endif + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) +void rtw_get_current_ip_address(struct adapter *padapter, u8 *pcurrentip) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct in_device *my_ip_ptr = padapter->pnetdev->ip_ptr; + u8 ipaddress[4]; + + if ((pmlmeinfo->state & WIFI_FW_LINKING_STATE) || + pmlmeinfo->state & WIFI_FW_AP_STATE) { + if (my_ip_ptr != NULL) { + struct in_ifaddr *my_ifa_list = my_ip_ptr->ifa_list; + if (my_ifa_list != NULL) { + ipaddress[0] = my_ifa_list->ifa_address & 0xFF; + ipaddress[1] = (my_ifa_list->ifa_address >> 8) & 0xFF; + ipaddress[2] = (my_ifa_list->ifa_address >> 16) & 0xFF; + ipaddress[3] = my_ifa_list->ifa_address >> 24; + DBG_871X("%s: %d.%d.%d.%d ==========\n", __func__, + ipaddress[0], ipaddress[1], ipaddress[2], ipaddress[3]); + memcpy(pcurrentip, ipaddress, 4); + } + } + } +} +#endif +#ifdef CONFIG_WOWLAN +void rtw_get_sec_iv(struct adapter *padapter, u8 *pcur_dot11txpn, u8 *StaAddr) +{ + struct sta_info *psta; + struct security_priv *psecpriv = &padapter->securitypriv; + + memset(pcur_dot11txpn, 0, 8); + if (NULL == StaAddr) + return; + psta = rtw_get_stainfo(&padapter->stapriv, StaAddr); + DBG_871X("%s(): StaAddr: %02x %02x %02x %02x %02x %02x\n", + __func__, StaAddr[0], StaAddr[1], StaAddr[2], + StaAddr[3], StaAddr[4], StaAddr[5]); + + if (psta) { + if (psecpriv->dot11PrivacyAlgrthm != _NO_PRIVACY_ && psta->dot11txpn.val > 0) + psta->dot11txpn.val--; + AES_IV(pcur_dot11txpn, psta->dot11txpn, 0); + + DBG_871X("%s(): CurrentIV: %02x %02x %02x %02x %02x %02x %02x %02x\n" + , __func__, pcur_dot11txpn[0], pcur_dot11txpn[1], + pcur_dot11txpn[2], pcur_dot11txpn[3], pcur_dot11txpn[4], + pcur_dot11txpn[5], pcur_dot11txpn[6], pcur_dot11txpn[7]); + } +} +void rtw_set_sec_pn(struct adapter *padapter) +{ + struct sta_info *psta; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct security_priv *psecpriv = &padapter->securitypriv; + + psta = rtw_get_stainfo(&padapter->stapriv, + get_my_bssid(&pmlmeinfo->network)); + + if (psta) { + if (pwrpriv->wowlan_fw_iv > psta->dot11txpn.val) { + if (psecpriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) + psta->dot11txpn.val = pwrpriv->wowlan_fw_iv + 2; + } else { + DBG_871X("%s(): FW IV is smaller than driver\n", __func__); + psta->dot11txpn.val += 2; + } + DBG_871X("%s: dot11txpn: 0x%016llx\n", __func__, psta->dot11txpn.val); + } +} +#endif /* CONFIG_WOWLAN */ + +#ifdef CONFIG_PNO_SUPPORT +#define CSCAN_TLV_TYPE_SSID_IE 'S' +#define CIPHER_IE "key_mgmt =" +#define CIPHER_NONE "NONE" +#define CIPHER_WPA_PSK "WPA-PSK" +#define CIPHER_WPA_EAP "WPA-EAP IEEE8021X" + +#endif /* CONFIG_PNO_SUPPORT */ diff --git a/drivers/staging/rtl8723bs/core/rtw_xmit.c b/drivers/staging/rtl8723bs/core/rtw_xmit.c new file mode 100644 index 0000000..8f2c9a6 --- /dev/null +++ b/drivers/staging/rtl8723bs/core/rtw_xmit.c @@ -0,0 +1,3100 @@ +/****************************************************************************** + * + * 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 <drv_types.h> +#include <rtw_debug.h> + +static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 }; +static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 }; + +static void _init_txservq(struct tx_servq *ptxservq) +{ + INIT_LIST_HEAD(&ptxservq->tx_pending); + _rtw_init_queue(&ptxservq->sta_pending); + ptxservq->qcnt = 0; +} + +void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv) +{ + memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv)); + + spin_lock_init(&psta_xmitpriv->lock); + + /* 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_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter) +{ + int i; + struct xmit_buf *pxmitbuf; + struct xmit_frame *pxframe; + sint res = _SUCCESS; + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */ + /* 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_queue below + */ + + pxmitpriv->adapter = padapter; + + /* for (i = 0 ; i < MAX_NUMBLKS; i++) */ + /* _rtw_init_queue(&pxmitpriv->blk_strms[i]); */ + + _rtw_init_queue(&pxmitpriv->be_pending); + _rtw_init_queue(&pxmitpriv->bk_pending); + _rtw_init_queue(&pxmitpriv->vi_pending); + _rtw_init_queue(&pxmitpriv->vo_pending); + _rtw_init_queue(&pxmitpriv->bm_pending); + + /* _rtw_init_queue(&pxmitpriv->legacy_dz_queue); */ + /* _rtw_init_queue(&pxmitpriv->apsd_queue); */ + + _rtw_init_queue(&pxmitpriv->free_xmit_queue); + + /* + Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME, + and initialize free_xmit_frame below. + Please also apply free_txobj to link_up all the xmit_frames... + */ + + pxmitpriv->pallocated_frame_buf = vzalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4); + + if (pxmitpriv->pallocated_frame_buf == NULL) { + pxmitpriv->pxmit_frame_buf = NULL; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n")); + res = _FAIL; + goto exit; + } + pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4); + /* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */ + /* ((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3); */ + + pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf; + + for (i = 0; i < NR_XMITFRAME; i++) { + INIT_LIST_HEAD(&(pxframe->list)); + + pxframe->padapter = padapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + list_add_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue)); + + pxframe++; + } + + pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME; + + pxmitpriv->frag_len = MAX_FRAG_THRESHOLD; + + + /* init xmit_buf */ + _rtw_init_queue(&pxmitpriv->free_xmitbuf_queue); + _rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue); + + pxmitpriv->pallocated_xmitbuf = vzalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4); + + if (pxmitpriv->pallocated_xmitbuf == NULL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_buf fail!\n")); + res = _FAIL; + goto exit; + } + + pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4); + /* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */ + /* ((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3); */ + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf; + + for (i = 0; i < NR_XMITBUFF; i++) { + INIT_LIST_HEAD(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_DATA; + + /* Tx buf allocation may fail sometimes, so sleep and retry. */ + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true); + if (res == _FAIL) { + msleep(10); + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true); + if (res == _FAIL) + goto exit; + } + + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + + pxmitbuf->flags = XMIT_VO_QUEUE; + + list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue)); + #ifdef DBG_XMIT_BUF + pxmitbuf->no = i; + #endif + + pxmitbuf++; + + } + + pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF; + + /* init xframe_ext queue, the same count as extbuf */ + _rtw_init_queue(&pxmitpriv->free_xframe_ext_queue); + + pxmitpriv->xframe_ext_alloc_addr = vzalloc(NR_XMIT_EXTBUFF * 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 = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4); + pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext; + + for (i = 0; i < NR_XMIT_EXTBUFF; 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 = NR_XMIT_EXTBUFF; + + /* Init xmit extension buff */ + _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue); + + pxmitpriv->pallocated_xmit_extbuf = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4); + + if (pxmitpriv->pallocated_xmit_extbuf == NULL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n")); + res = _FAIL; + goto exit; + } + + pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4); + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + INIT_LIST_HEAD(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_MGNT; + + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ, true); + if (res == _FAIL) { + res = _FAIL; + goto exit; + } + + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMIT_EXTBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + + list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue)); + #ifdef DBG_XMIT_BUF_EXT + pxmitbuf->no = i; + #endif + pxmitbuf++; + + } + + pxmitpriv->free_xmit_extbuf_cnt = NR_XMIT_EXTBUFF; + + for (i = 0; i < CMDBUF_MAX; i++) { + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i]; + if (pxmitbuf) { + INIT_LIST_HEAD(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_CMD; + + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true); + if (res == _FAIL) { + res = _FAIL; + goto exit; + } + + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_CMDBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->alloc_sz = MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ; + } + } + + rtw_alloc_hwxmits(padapter); + rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry); + + for (i = 0; i < 4; i++) { + pxmitpriv->wmm_para_seq[i] = i; + } + + pxmitpriv->ack_tx = false; + mutex_init(&pxmitpriv->ack_tx_mutex); + rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0); + + rtw_hal_init_xmit_priv(padapter); + +exit: + return res; +} + +void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv) +{ + int i; + struct adapter *padapter = pxmitpriv->adapter; + struct xmit_frame *pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf; + struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf; + + rtw_hal_free_xmit_priv(padapter); + + if (pxmitpriv->pxmit_frame_buf == NULL) + return; + + for (i = 0; i < NR_XMITFRAME; i++) { + rtw_os_xmit_complete(padapter, pxmitframe); + + pxmitframe++; + } + + for (i = 0; i < NR_XMITBUFF; i++) { + rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true); + + pxmitbuf++; + } + + if (pxmitpriv->pallocated_frame_buf) + vfree(pxmitpriv->pallocated_frame_buf); + + + if (pxmitpriv->pallocated_xmitbuf) + vfree(pxmitpriv->pallocated_xmitbuf); + + /* free xframe_ext queue, the same count as extbuf */ + pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext; + if (pxmitframe) { + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + rtw_os_xmit_complete(padapter, pxmitframe); + pxmitframe++; + } + } + if (pxmitpriv->xframe_ext_alloc_addr) + vfree(pxmitpriv->xframe_ext_alloc_addr); + + /* free xmit extension buff */ + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ), true); + + pxmitbuf++; + } + + if (pxmitpriv->pallocated_xmit_extbuf) { + vfree(pxmitpriv->pallocated_xmit_extbuf); + } + + for (i = 0; i < CMDBUF_MAX; i++) { + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i]; + if (pxmitbuf != NULL) + rtw_os_xmit_resource_free(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true); + } + + rtw_free_hwxmits(padapter); + + mutex_destroy(&pxmitpriv->ack_tx_mutex); +} + +u8 query_ra_short_GI(struct sta_info *psta) +{ + u8 sgi = false, sgi_20m = false, sgi_40m = false, sgi_80m = false; + + sgi_20m = psta->htpriv.sgi_20m; + sgi_40m = psta->htpriv.sgi_40m; + + switch (psta->bw_mode) { + case CHANNEL_WIDTH_80: + sgi = sgi_80m; + break; + case CHANNEL_WIDTH_40: + sgi = sgi_40m; + break; + case CHANNEL_WIDTH_20: + default: + sgi = sgi_20m; + break; + } + + return sgi; +} + +static void update_attrib_vcs_info(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + u32 sz; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + /* struct sta_info *psta = pattrib->psta; */ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pattrib->nr_frags != 1) + sz = padapter->xmitpriv.frag_len; + else /* no frag */ + sz = pattrib->last_txcmdsz; + + /* (1) RTS_Threshold is compared to the MPDU, not MSDU. */ + /* (2) If there are more than one frag in this MSDU, only the first frag uses protection frame. */ + /* Other fragments are protected by previous fragment. */ + /* So we only need to check the length of first fragment. */ + if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N || padapter->registrypriv.wifi_spec) { + if (sz > padapter->registrypriv.rts_thresh) + pattrib->vcs_mode = RTS_CTS; + else{ + if (pattrib->rtsen) + pattrib->vcs_mode = RTS_CTS; + else if (pattrib->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 == true) && + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + pattrib->vcs_mode = CTS_TO_SELF; + break; + } + + + /* check ERP protection */ + if (pattrib->rtsen || pattrib->cts2self) { + if (pattrib->rtsen) + pattrib->vcs_mode = RTS_CTS; + else if (pattrib->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 == true) { + pattrib->vcs_mode = RTS_CTS; + break; + } + + pattrib->vcs_mode = NONE_VCS; + break; + } + } + + /* for debug : force driver control vrtl_carrier_sense. */ + if (padapter->driver_vcs_en == 1) + pattrib->vcs_mode = padapter->driver_vcs_type; +} + +static void update_attrib_phy_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta) +{ + struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv; + + pattrib->rtsen = psta->rtsen; + pattrib->cts2self = psta->cts2self; + + pattrib->mdata = 0; + pattrib->eosp = 0; + pattrib->triggered = 0; + pattrib->ampdu_spacing = 0; + + /* qos_en, ht_en, init rate, , bw, ch_offset, sgi */ + pattrib->qos_en = psta->qos_option; + + pattrib->raid = psta->raid; + + if (mlmeext->cur_bwmode < psta->bw_mode) + pattrib->bwmode = mlmeext->cur_bwmode; + else + pattrib->bwmode = psta->bw_mode; + + pattrib->sgi = query_ra_short_GI(psta); + + pattrib->ldpc = psta->ldpc; + pattrib->stbc = psta->stbc; + + pattrib->ht_en = psta->htpriv.ht_option; + pattrib->ch_offset = psta->htpriv.ch_offset; + pattrib->ampdu_en = false; + + if (padapter->driver_ampdu_spacing != 0xFF) /* driver control AMPDU Density for peer sta's rx */ + pattrib->ampdu_spacing = padapter->driver_ampdu_spacing; + else + pattrib->ampdu_spacing = psta->htpriv.rx_ampdu_min_spacing; + + /* if (pattrib->ht_en && psta->htpriv.ampdu_enable) */ + /* */ + /* if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */ + /* pattrib->ampdu_en = true; */ + /* */ + + + pattrib->retry_ctrl = false; + +#ifdef CONFIG_AUTO_AP_MODE + if (psta->isrc && psta->pid > 0) + pattrib->pctrl = true; +#endif + +} + +static s32 update_attrib_sec_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta) +{ + sint res = _SUCCESS; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + sint bmcast = IS_MCAST(pattrib->ra); + + memset(pattrib->dot118021x_UncstKey.skey, 0, 16); + memset(pattrib->dot11tkiptxmickey.skey, 0, 16); + pattrib->mac_id = psta->mac_id; + + 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)); + #ifdef DBG_TX_DROP_FRAME + DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == true, pattrib->ether_type(%04x) != 0x888e\n", __func__, pattrib->ether_type); + #endif + 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; + } + + /* For WPS 1.0 WEP, driver should not encrypt EAPOL Packet for WPS handshake. */ + if (((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) && (pattrib->ether_type == 0x888e)) + pattrib->encrypt = _NO_PRIVACY_; + + } + + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + pattrib->iv_len = 4; + pattrib->icv_len = 4; + WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + break; + + case _TKIP_: + pattrib->iv_len = 8; + pattrib->icv_len = 4; + + if (psecuritypriv->busetkipkey == _FAIL) { + #ifdef DBG_TX_DROP_FRAME + DBG_871X("DBG_TX_DROP_FRAME %s psecuritypriv->busetkipkey(%d) == _FAIL drop packet\n", __func__, psecuritypriv->busetkipkey); + #endif + res = _FAIL; + goto exit; + } + + if (bmcast) + TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + TKIP_IV(pattrib->iv, psta->dot11txpn, 0); + + + memcpy(pattrib->dot11tkiptxmickey.skey, psta->dot11tkiptxmickey.skey, 16); + + break; + + case _AES_: + + pattrib->iv_len = 8; + pattrib->icv_len = 8; + + if (bmcast) + AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + AES_IV(pattrib->iv, psta->dot11txpn, 0); + + break; + + default: + pattrib->iv_len = 0; + pattrib->icv_len = 0; + break; + } + + if (pattrib->encrypt > 0) + memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16); + + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, + ("update_attrib: encrypt =%d securitypriv.sw_encrypt =%d\n", + pattrib->encrypt, padapter->securitypriv.sw_encrypt)); + + if (pattrib->encrypt && + ((padapter->securitypriv.sw_encrypt == true) || (psecuritypriv->hw_decrypted == false))) { + pattrib->bswenc = true; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("update_attrib: encrypt =%d securitypriv.hw_decrypted =%d bswenc =true\n", + pattrib->encrypt, padapter->securitypriv.sw_encrypt)); + } else { + pattrib->bswenc = false; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc =false\n")); + } + +exit: + + return res; + +} + +u8 qos_acm(u8 acm_mask, u8 priority) +{ + u8 change_priority = priority; + + switch (priority) { + case 0: + case 3: + if (acm_mask & BIT(1)) + change_priority = 1; + break; + case 1: + case 2: + break; + case 4: + case 5: + if (acm_mask & BIT(2)) + change_priority = 0; + break; + case 6: + case 7: + if (acm_mask & BIT(3)) + change_priority = 5; + break; + default: + DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority); + break; + } + + return change_priority; +} + +static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib) +{ + struct ethhdr etherhdr; + struct iphdr ip_hdr; + s32 UserPriority = 0; + + + _rtw_open_pktfile(ppktfile->pkt, ppktfile); + _rtw_pktfile_read(ppktfile, (unsigned char *)ðerhdr, ETH_HLEN); + + /* get UserPriority from IP hdr */ + if (pattrib->ether_type == 0x0800) { + _rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr)); +/* UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */ + UserPriority = ip_hdr.tos >> 5; + } + pattrib->priority = UserPriority; + pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN; + pattrib->subtype = WIFI_QOS_DATA_TYPE; +} + +static s32 update_attrib(struct adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib) +{ + uint i; + struct pkt_file pktfile; + struct sta_info *psta = NULL; + struct ethhdr etherhdr; + + sint bmcast; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + sint res = _SUCCESS; + + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib); + + _rtw_open_pktfile(pkt, &pktfile); + i = _rtw_pktfile_read(&pktfile, (u8 *)ðerhdr, ETH_HLEN); + + pattrib->ether_type = ntohs(etherhdr.h_proto); + + + memcpy(pattrib->dst, ðerhdr.h_dest, ETH_ALEN); + memcpy(pattrib->src, ðerhdr.h_source, ETH_ALEN); + + + 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); + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc); + } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta); + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN); + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_ap); + } else + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown); + + pattrib->pktlen = pktfile.pkt_len; + + if (ETH_P_IP == pattrib->ether_type) { + /* The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */ + /* to prevent DHCP protocol fail */ + + u8 tmp[24]; + + _rtw_pktfile_read(&pktfile, &tmp[0], 24); + + pattrib->dhcp_pkt = 0; + if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */ + if (ETH_P_IP == pattrib->ether_type) {/* IP header */ + if (((tmp[21] == 68) && (tmp[23] == 67)) || + ((tmp[21] == 67) && (tmp[23] == 68))) { + /* 68 : UDP BOOTP client */ + /* 67 : UDP BOOTP server */ + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======================update_attrib: get DHCP Packet\n")); + pattrib->dhcp_pkt = 1; + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp); + } + } + } + + /* for parsing ICMP pakcets */ + { + struct iphdr *piphdr = (struct iphdr *)tmp; + + pattrib->icmp_pkt = 0; + if (piphdr->protocol == 0x1) { /* protocol type in ip header 0x1 is ICMP */ + pattrib->icmp_pkt = 1; + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp); + } + } + + + } else if (0x888e == pattrib->ether_type) { + DBG_871X_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->icmp_pkt == 1) + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1); + else if (pattrib->dhcp_pkt == 1) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active); + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1); + } + + bmcast = IS_MCAST(pattrib->ra); + + /* get sta_info */ + if (bmcast) { + psta = rtw_get_bcmc_stainfo(padapter); + } else { + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + if (psta == NULL) { /* if we cannot get psta => drop the pkt */ + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra))); + #ifdef DBG_TX_DROP_FRAME + DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra)); + #endif + res = _FAIL; + goto exit; + } else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link); + res = _FAIL; + goto exit; + } + } + + if (psta == NULL) { + /* if we cannot get psta => drop the pkt */ + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra))); + #ifdef DBG_TX_DROP_FRAME + DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra)); + #endif + res = _FAIL; + goto exit; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link); + DBG_871X("%s, psta("MAC_FMT")->state(0x%x) != _FW_LINKED\n", __func__, MAC_ARG(psta->hwaddr), psta->state); + return _FAIL; + } + + + + /* TODO:_lock */ + if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sec); + res = _FAIL; + goto exit; + } + + update_attrib_phy_info(padapter, pattrib, psta); + + /* DBG_8192C("%s ==> mac_id(%d)\n", __func__, pattrib->mac_id); */ + + pattrib->psta = psta; + /* TODO:_unlock */ + + pattrib->pctrl = 0; + + pattrib->ack_policy = 0; + /* get ether_hdr_len */ + pattrib->pkt_hdrlen = ETH_HLEN;/* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */ + + pattrib->hdrlen = WLAN_HDR_A3_LEN; + pattrib->subtype = WIFI_DATA_TYPE; + pattrib->priority = 0; + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) { + if (pattrib->qos_en) + set_qos(&pktfile, pattrib); + } else{ + if (pqospriv->qos_option) { + set_qos(&pktfile, pattrib); + + if (pmlmepriv->acm_mask != 0) + pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority); + + } + } + + /* pattrib->priority = 5; force to used VI queue, for testing */ + + rtw_set_tx_chksum_offload(pkt, pattrib); + +exit: + return res; +} + +static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + sint curfragnum, length; + u8 *pframe, *payload, mic[8]; + struct mic_data micdata; + /* struct sta_info *stainfo; */ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u8 priority[4] = {0x0, 0x0, 0x0, 0x0}; + u8 hw_hdr_offset = 0; + sint bmcst = IS_MCAST(pattrib->ra); + +/* + if (pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + DBG_871X("%s, call rtw_get_stainfo()\n", __func__); + stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]); + } + + if (stainfo == NULL) + { + DBG_871X("%s, psta ==NUL\n", __func__); + return _FAIL; + } + + if (!(stainfo->state &_FW_LINKED)) + { + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state); + return _FAIL; + } +*/ + + hw_hdr_offset = TXDESC_OFFSET; + + if (pattrib->encrypt == _TKIP_) { /* if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_PRIVACY_) */ + /* encode mic code */ + /* if (stainfo!= NULL) */ + { + u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + + pframe = pxmitframe->buf_addr + hw_hdr_offset; + + if (bmcst) { + if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)) { + /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */ + /* msleep(10); */ + return _FAIL; + } + /* start to calculate the mic code */ + rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey); + } else { + if (!memcmp(&pattrib->dot11tkiptxmickey.skey[0], null_key, 16)) { + /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */ + /* msleep(10); */ + return _FAIL; + } + /* start to calculate the mic code */ + rtw_secmicsetkey(&micdata, &pattrib->dot11tkiptxmickey.skey[0]); + } + + if (pframe[1]&1) { /* ToDS == 1 */ + rtw_secmicappend(&micdata, &pframe[16], 6); /* DA */ + if (pframe[1]&2) /* From Ds == 1 */ + rtw_secmicappend(&micdata, &pframe[24], 6); + else + rtw_secmicappend(&micdata, &pframe[10], 6); + } else { /* ToDS == 0 */ + rtw_secmicappend(&micdata, &pframe[4], 6); /* DA */ + if (pframe[1]&2) /* From Ds == 1 */ + rtw_secmicappend(&micdata, &pframe[16], 6); + else + rtw_secmicappend(&micdata, &pframe[10], 6); + + } + + /* if (pqospriv->qos_option == 1) */ + if (pattrib->qos_en) + priority[0] = (u8)pxmitframe->attrib.priority; + + + rtw_secmicappend(&micdata, &priority[0], 4); + + payload = pframe; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + payload = (u8 *)RND4((SIZE_PTR)(payload)); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("===curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n", + curfragnum, *payload, *(payload+1), *(payload+2), *(payload+3), *(payload+4), *(payload+5), *(payload+6), *(payload+7))); + + payload = payload+pattrib->hdrlen+pattrib->iv_len; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d", curfragnum, pattrib->hdrlen, pattrib->iv_len)); + if ((curfragnum+1) == pattrib->nr_frags) { + length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0); + rtw_secmicappend(&micdata, payload, length); + payload = payload+length; + } else{ + length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0); + rtw_secmicappend(&micdata, payload, length); + payload = payload+length+pattrib->icv_len; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d length =%d pattrib->icv_len =%d", curfragnum, length, pattrib->icv_len)); + } + } + rtw_secgetmic(&micdata, &(mic[0])); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: before add mic code!!!\n")); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n", pattrib->last_txcmdsz)); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: mic[0]= 0x%.2x , mic[1]= 0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\n\ + mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n", + mic[0], mic[1], mic[2], mic[3], mic[4], mic[5], mic[6], mic[7])); + /* add mic code and add the mic code length in last_txcmdsz */ + + memcpy(payload, &(mic[0]), 8); + pattrib->last_txcmdsz += 8; + + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("\n ========last pkt ========\n")); + payload = payload-pattrib->last_txcmdsz+8; + for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz; curfragnum = curfragnum+8) + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, (" %.2x, %.2x, %.2x, %.2x, %.2x, %.2x, %.2x, %.2x ", + *(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2), *(payload+curfragnum+3), + *(payload+curfragnum+4), *(payload+curfragnum+5), *(payload+curfragnum+6), *(payload+curfragnum+7))); + } +/* + else { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: rtw_get_stainfo == NULL!!!\n")); + } +*/ + } + return _SUCCESS; +} + +static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + /* struct security_priv *psecuritypriv =&padapter->securitypriv; */ + + /* if ((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */ + if (pattrib->bswenc) { + /* DBG_871X("start xmitframe_swencrypt\n"); */ + RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("### xmitframe_swencrypt\n")); + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + rtw_wep_encrypt(padapter, (u8 *)pxmitframe); + break; + case _TKIP_: + rtw_tkip_encrypt(padapter, (u8 *)pxmitframe); + break; + case _AES_: + rtw_aes_encrypt(padapter, (u8 *)pxmitframe); + break; + default: + break; + } + + } else + RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, ("### xmitframe_hwencrypt\n")); + + return _SUCCESS; +} + +s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib) +{ + u16 *qc; + + struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + u8 qos_option = false; + sint res = _SUCCESS; + __le16 *fctrl = &pwlanhdr->frame_control; + + 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; */ + + { + /* 1.Data transfer to AP */ + /* 2.Arp pkt will relayed by 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 (pattrib->qos_en) + 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 (pattrib->qos_en) + 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 */ + { + struct sta_info *psta; + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FAIL; + } + + if (psta == NULL) { + DBG_871X("%s, psta ==NUL\n", __func__); + return _FAIL; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return _FAIL; + } + + + if (psta) { + psta->sta_xmitpriv.txseq_tid[pattrib->priority]++; + psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF; + pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority]; + + SetSeqNum(hdr, pattrib->seqnum); + + /* check if enable ampdu */ + if (pattrib->ht_en && psta->htpriv.ampdu_enable) + if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) + pattrib->ampdu_en = true; + + + /* re-check if enable ampdu by BA_starting_seqctrl */ + if (pattrib->ampdu_en == true) { + u16 tx_seq; + + tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f]; + + /* check BA_starting_seqctrl */ + if (SN_LESS(pattrib->seqnum, tx_seq)) { + /* DBG_871X("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_871X("tx ampdu over run\n"); */ + psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff; + pattrib->ampdu_en = true;/* AGG EN */ + } + + } + } + } + + } else{ + + } + +exit: + return res; +} + +s32 rtw_txframes_pending(struct adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + return ((!list_empty(&pxmitpriv->be_pending.queue)) || + (!list_empty(&pxmitpriv->bk_pending.queue)) || + (!list_empty(&pxmitpriv->vi_pending.queue)) || + (!list_empty(&pxmitpriv->vo_pending.queue))); +} + +/* + * Calculate wlan 802.11 packet MAX size from pkt_attrib + * This function doesn't consider fragment case + */ +u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib) +{ + u32 len = 0; + + len = pattrib->hdrlen + pattrib->iv_len; /* WLAN Header and IV */ + len += SNAP_SIZE + sizeof(u16); /* LLC */ + len += pattrib->pktlen; + if (pattrib->encrypt == _TKIP_) + len += 8; /* MIC */ + len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /* ICV */ + + return len; +} + +/* + +This sub-routine will perform all the following: + +1. remove 802.3 header. +2. create wlan_header, based on the info in pxmitframe +3. append sta's iv/ext-iv +4. append LLC +5. move frag chunk from pframe to pxmitframe->mem +6. apply sw-encrypt, if necessary. + +*/ +s32 rtw_xmitframe_coalesce(struct adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe) +{ + struct pkt_file pktfile; + + s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz; + + SIZE_PTR addr; + + u8 *pframe, *mem_start; + u8 hw_hdr_offset; + + /* struct sta_info *psta; */ + /* struct sta_priv *pstapriv = &padapter->stapriv; */ + /* struct mlme_priv *pmlmepriv = &padapter->mlmepriv; */ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + u8 *pbuf_start; + + s32 bmcst = IS_MCAST(pattrib->ra); + s32 res = _SUCCESS; + +/* + if (pattrib->psta) + { + psta = pattrib->psta; + } else + { + DBG_871X("%s, call rtw_get_stainfo()\n", __func__); + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + } + + if (psta == NULL) + { + + DBG_871X("%s, psta ==NUL\n", __func__); + return _FAIL; + } + + + if (!(psta->state &_FW_LINKED)) + { + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return _FAIL; + } +*/ + if (pxmitframe->buf_addr == NULL) { + DBG_8192C("==> %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_wlanhdr(padapter, mem_start, pattrib) == _FAIL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n")); + DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"); + res = _FAIL; + goto exit; + } + + _rtw_open_pktfile(pkt, &pktfile); + _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen); + + frg_inx = 0; + frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */ + + while (1) { + llc_sz = 0; + + mpdu_len = frg_len; + + pframe = mem_start; + + SetMFrag(mem_start); + + pframe += pattrib->hdrlen; + mpdu_len -= pattrib->hdrlen; + + /* adding icv, if necessary... */ + if (pattrib->iv_len) { + memcpy(pframe, pattrib->iv, pattrib->iv_len); + + RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, + ("rtw_xmitframe_coalesce: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n", + padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3))); + + pframe += pattrib->iv_len; + + mpdu_len -= pattrib->iv_len; + } + + if (frg_inx == 0) { + llc_sz = rtw_put_snap(pframe, pattrib->ether_type); + pframe += llc_sz; + mpdu_len -= llc_sz; + } + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + mpdu_len -= pattrib->icv_len; + } + + + if (bmcst) { + /* don't do fragment to broadcat/multicast packets */ + mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen); + } else { + mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len); + } + + pframe += mem_sz; + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + + frg_inx++; + + if (bmcst || (rtw_endofpktfile(&pktfile) == true)) { + pattrib->nr_frags = frg_inx; + + pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz:0) + + ((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz; + + ClearMFrag(mem_start); + + break; + } else + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__)); + + addr = (SIZE_PTR)(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_8192C("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; +} + +/* broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption */ +s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe) +{ + u8 *pframe, *mem_start = NULL, *tmp_buf = NULL; + u8 subtype; + struct sta_info *psta = NULL; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + s32 bmcst = IS_MCAST(pattrib->ra); + u8 *BIP_AAD = NULL; + u8 *MGMT_body = NULL; + + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ieee80211_hdr *pwlanhdr; + u8 MME[_MME_IE_LENGTH_]; + u32 ori_len; + mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + ori_len = BIP_AAD_SIZE+pattrib->pktlen; + tmp_buf = BIP_AAD = rtw_zmalloc(ori_len); + subtype = GetFrameSubType(pframe); /* bit(7)~bit(2) */ + + if (BIP_AAD == NULL) + return _FAIL; + + spin_lock_bh(&padapter->security_key_mutex); + + /* only support station mode */ + if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED)) + goto xmitframe_coalesce_success; + + /* IGTK key is not install, it may not support 802.11w */ + if (padapter->securitypriv.binstallBIPkey != true) { + DBG_871X("no instll BIP key\n"); + goto xmitframe_coalesce_success; + } + /* station mode doesn't need TX BIP, just ready the code */ + if (bmcst) { + int frame_body_len; + u8 mic[16]; + + memset(MME, 0, 18); + + /* other types doesn't need the BIP */ + if (GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC) + goto xmitframe_coalesce_fail; + + MGMT_body = pframe + sizeof(struct ieee80211_hdr_3addr); + pframe += pattrib->pktlen; + + /* octent 0 and 1 is key index , BIP keyid is 4 or 5, LSB only need octent 0 */ + MME[0] = padapter->securitypriv.dot11wBIPKeyid; + /* copy packet number */ + memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6); + /* increase the packet number */ + pmlmeext->mgnt_80211w_IPN++; + + /* add MME IE with MIC all zero, MME string doesn't include element id and length */ + pframe = rtw_set_ie(pframe, _MME_IE_, 16, MME, &(pattrib->pktlen)); + pattrib->last_txcmdsz = pattrib->pktlen; + /* total frame length - header length */ + frame_body_len = pattrib->pktlen - sizeof(struct ieee80211_hdr_3addr); + + /* conscruct AAD, copy frame control field */ + memcpy(BIP_AAD, &pwlanhdr->frame_control, 2); + ClearRetry(BIP_AAD); + ClearPwrMgt(BIP_AAD); + ClearMData(BIP_AAD); + /* conscruct AAD, copy address 1 to address 3 */ + memcpy(BIP_AAD+2, pwlanhdr->addr1, 18); + /* copy management fram body */ + memcpy(BIP_AAD+BIP_AAD_SIZE, MGMT_body, frame_body_len); + /* calculate mic */ + if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey + , BIP_AAD, BIP_AAD_SIZE+frame_body_len, mic)) + goto xmitframe_coalesce_fail; + + /* copy right BIP mic value, total is 128bits, we use the 0~63 bits */ + memcpy(pframe-8, mic, 8); + } else { /* unicast mgmt frame TX */ + /* start to encrypt mgmt frame */ + if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC || + subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION) { + if (pattrib->psta) + psta = pattrib->psta; + else + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + + if (psta == NULL) { + + DBG_871X("%s, psta ==NUL\n", __func__); + goto xmitframe_coalesce_fail; + } + + if (!(psta->state & _FW_LINKED) || pxmitframe->buf_addr == NULL) { + DBG_871X("%s, not _FW_LINKED or addr null\n", __func__); + goto xmitframe_coalesce_fail; + } + + /* DBG_871X("%s, action frame category =%d\n", __func__, pframe[WLAN_HDR_A3_LEN]); */ + /* according 802.11-2012 standard, these five types are not robust types */ + if (subtype == WIFI_ACTION && + (pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC || + pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT || + pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM || + pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED || + pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P)) + goto xmitframe_coalesce_fail; + /* before encrypt dump the management packet content */ + if (pattrib->encrypt > 0) + memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16); + /* bakeup original management packet */ + memcpy(tmp_buf, pframe, pattrib->pktlen); + /* move to data portion */ + pframe += pattrib->hdrlen; + + /* 802.11w unicast management packet must be _AES_ */ + pattrib->iv_len = 8; + /* it's MIC of AES */ + pattrib->icv_len = 8; + + switch (pattrib->encrypt) { + case _AES_: + /* set AES IV header */ + AES_IV(pattrib->iv, psta->dot11wtxpn, 0); + break; + default: + goto xmitframe_coalesce_fail; + } + /* insert iv header into management frame */ + memcpy(pframe, pattrib->iv, pattrib->iv_len); + pframe += pattrib->iv_len; + /* copy mgmt data portion after CCMP header */ + memcpy(pframe, tmp_buf+pattrib->hdrlen, pattrib->pktlen-pattrib->hdrlen); + /* move pframe to end of mgmt pkt */ + pframe += pattrib->pktlen-pattrib->hdrlen; + /* add 8 bytes CCMP IV header to length */ + pattrib->pktlen += pattrib->iv_len; + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + /* add 8 bytes MIC */ + pattrib->pktlen += pattrib->icv_len; + /* set final tx command size */ + pattrib->last_txcmdsz = pattrib->pktlen; + + /* set protected bit must be beofre SW encrypt */ + SetPrivacy(mem_start); + /* software encrypt */ + xmitframe_swencrypt(padapter, pxmitframe); + } + } + +xmitframe_coalesce_success: + spin_unlock_bh(&padapter->security_key_mutex); + kfree(BIP_AAD); + return _SUCCESS; + +xmitframe_coalesce_fail: + spin_unlock_bh(&padapter->security_key_mutex); + kfree(BIP_AAD); + return _FAIL; +} + +/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header + * IEEE LLC/SNAP header contains 8 octets + * First 3 octets comprise the LLC portion + * SNAP portion, 5 octets, is divided into two fields: + *Organizationally Unique Identifier(OUI), 3 octets, + *type, defined by that organization, 2 octets. + */ +s32 rtw_put_snap(u8 *data, u16 h_proto) +{ + struct ieee80211_snap_hdr *snap; + u8 *oui; + + snap = (struct ieee80211_snap_hdr *)data; + snap->dsap = 0xaa; + snap->ssap = 0xaa; + snap->ctrl = 0x03; + + if (h_proto == 0x8137 || h_proto == 0x80f3) + oui = P802_1H_OUI; + else + oui = RFC1042_OUI; + + snap->oui[0] = oui[0]; + snap->oui[1] = oui[1]; + snap->oui[2] = oui[2]; + + *(__be16 *)(data + SNAP_SIZE) = htons(h_proto); + + return SNAP_SIZE + sizeof(u16); +} + +void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len) +{ + + uint protection; + u8 *perp; + sint erp_len; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + + switch (pxmitpriv->vcs_setting) { + case DISABLE_VCS: + pxmitpriv->vcs = NONE_VCS; + break; + + case ENABLE_VCS: + break; + + case AUTO_VCS: + default: + perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len); + if (perp == NULL) + pxmitpriv->vcs = NONE_VCS; + else{ + protection = (*(perp + 2)) & BIT(1); + if (protection) { + if (pregistrypriv->vcs_type == RTS_CTS) + pxmitpriv->vcs = RTS_CTS; + else + pxmitpriv->vcs = CTS_TO_SELF; + } else + pxmitpriv->vcs = NONE_VCS; + } + + break; + + } +} + +void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz) +{ + struct sta_info *psta = NULL; + struct stainfo_stats *pstats = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 pkt_num = 1; + + if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) { + pkt_num = pxmitframe->agg_num; + + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pkt_num; + + pxmitpriv->tx_pkts += pkt_num; + + pxmitpriv->tx_bytes += sz; + + psta = pxmitframe->attrib.psta; + if (psta) { + pstats = &psta->sta_stats; + + pstats->tx_pkts += pkt_num; + + pstats->tx_bytes += sz; + } + } +} + +static struct xmit_buf *__rtw_alloc_cmd_xmitbuf(struct xmit_priv *pxmitpriv, + enum cmdbuf_type buf_type) +{ + struct xmit_buf *pxmitbuf = NULL; + + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[buf_type]; + if (pxmitbuf != NULL) { + pxmitbuf->priv_data = NULL; + + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 0; + pxmitbuf->pg_num = 0; + + if (pxmitbuf->sctx) { + DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } else + DBG_871X("%s fail, no xmitbuf available !!!\n", __func__); + + return pxmitbuf; +} + +struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv, + enum cmdbuf_type buf_type) +{ + struct xmit_frame *pcmdframe; + struct xmit_buf *pxmitbuf; + + pcmdframe = rtw_alloc_xmitframe(pxmitpriv); + if (pcmdframe == NULL) { + DBG_871X("%s, alloc xmitframe fail\n", __func__); + return NULL; + } + + pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type); + if (pxmitbuf == NULL) { + DBG_871X("%s, alloc xmitbuf fail\n", __func__); + rtw_free_xmitframe(pxmitpriv, pcmdframe); + return NULL; + } + + pcmdframe->frame_tag = MGNT_FRAMETAG; + + pcmdframe->pxmitbuf = pxmitbuf; + + pcmdframe->buf_addr = pxmitbuf->pbuf; + + pxmitbuf->priv_data = pcmdframe; + + return pcmdframe; + +} + +struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv) +{ + _irqL irqL; + struct xmit_buf *pxmitbuf = NULL; + struct list_head *plist, *phead; + struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue; + + spin_lock_irqsave(&pfree_queue->lock, irqL); + + if (list_empty(&pfree_queue->queue)) { + pxmitbuf = NULL; + } else { + + phead = get_list_head(pfree_queue); + + plist = get_next(phead); + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + list_del_init(&(pxmitbuf->list)); + } + + if (pxmitbuf != NULL) { + pxmitpriv->free_xmit_extbuf_cnt--; + #ifdef DBG_XMIT_BUF_EXT + DBG_871X("DBG_XMIT_BUF_EXT ALLOC no =%d, free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt); + #endif + + + pxmitbuf->priv_data = NULL; + + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 1; + + if (pxmitbuf->sctx) { + DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + + } + + spin_unlock_irqrestore(&pfree_queue->lock, irqL); + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + _irqL irqL; + struct __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++; + #ifdef DBG_XMIT_BUF_EXT + DBG_871X("DBG_XMIT_BUF_EXT FREE no =%d, free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt); + #endif + + spin_unlock_irqrestore(&pfree_queue->lock, irqL); + + return _SUCCESS; +} + +struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv) +{ + _irqL irqL; + struct xmit_buf *pxmitbuf = NULL; + struct list_head *plist, *phead; + struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + /* DBG_871X("+rtw_alloc_xmitbuf\n"); */ + + spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL); + + if (list_empty(&pfree_xmitbuf_queue->queue)) { + pxmitbuf = NULL; + } else { + + phead = get_list_head(pfree_xmitbuf_queue); + + plist = get_next(phead); + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + list_del_init(&(pxmitbuf->list)); + } + + if (pxmitbuf != NULL) { + pxmitpriv->free_xmitbuf_cnt--; + #ifdef DBG_XMIT_BUF + DBG_871X("DBG_XMIT_BUF ALLOC no =%d, free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt); + #endif + /* DBG_871X("alloc, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */ + + pxmitbuf->priv_data = NULL; + + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 0; + pxmitbuf->pg_num = 0; + + if (pxmitbuf->sctx) { + DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } + #ifdef DBG_XMIT_BUF + else + DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n"); + #endif + + spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL); + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + _irqL irqL; + struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + /* DBG_871X("+rtw_free_xmitbuf\n"); */ + + if (pxmitbuf == NULL) + return _FAIL; + + if (pxmitbuf->sctx) { + DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE); + } + + if (pxmitbuf->buf_tag == XMITBUF_CMD) { + } else if (pxmitbuf->buf_tag == XMITBUF_MGNT) { + rtw_free_xmitbuf_ext(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++; + /* DBG_871X("FREE, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */ + #ifdef DBG_XMIT_BUF + DBG_871X("DBG_XMIT_BUF FREE no =%d, free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt); + #endif + 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->pg_num = 1; + pxframe->agg_num = 1; + 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_xmitframe(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */ +{ + /* + Please remember to use all the osdep_service api, + and lock/unlock or _enter/_exit critical to protect + pfree_xmit_queue + */ + + struct xmit_frame *pxframe = NULL; + struct list_head *plist, *phead; + struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue; + + spin_lock_bh(&pfree_xmit_queue->lock); + + if (list_empty(&pfree_xmit_queue->queue)) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt)); + pxframe = NULL; + } else { + phead = get_list_head(pfree_xmit_queue); + + plist = get_next(phead); + + pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + list_del_init(&(pxframe->list)); + pxmitpriv->free_xmitframe_cnt--; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():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_xmitframe_ext(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pxframe = NULL; + struct list_head *plist, *phead; + struct __queue *queue = &pxmitpriv->free_xframe_ext_queue; + + spin_lock_bh(&queue->lock); + + if (list_empty(&queue->queue)) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt)); + pxframe = NULL; + } else { + phead = get_list_head(queue); + plist = get_next(phead); + pxframe = LIST_CONTAINOR(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_xmitframe_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt)); + } + + spin_unlock_bh(&queue->lock); + + rtw_init_xmitframe(pxframe); + + return pxframe; +} + +struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pxframe = NULL; + u8 *alloc_addr; + + alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4); + + if (alloc_addr == NULL) + goto exit; + + pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4); + pxframe->alloc_addr = alloc_addr; + + pxframe->padapter = pxmitpriv->adapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + rtw_init_xmitframe(pxframe); + + DBG_871X("################## %s ##################\n", __func__); + +exit: + return pxframe; +} + +s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe) +{ + struct __queue *queue = NULL; + struct adapter *padapter = pxmitpriv->adapter; + _pkt *pndis_pkt = NULL; + + if (pxmitframe == NULL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe == NULL!!!!!!!!!!\n")); + goto exit; + } + + if (pxmitframe->pkt) { + pndis_pkt = pxmitframe->pkt; + pxmitframe->pkt = NULL; + } + + if (pxmitframe->alloc_addr) { + DBG_871X("################## %s with alloc_addr ##################\n", __func__); + kfree(pxmitframe->alloc_addr); + goto check_pkt_complete; + } + + if (pxmitframe->ext_tag == 0) + queue = &pxmitpriv->free_xmit_queue; + else if (pxmitframe->ext_tag == 1) + queue = &pxmitpriv->free_xframe_ext_queue; + else { + + } + + 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_xmitframe():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_xmitframe():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt)); + } else { + } + + spin_unlock_bh(&queue->lock); + +check_pkt_complete: + + if (pndis_pkt) + rtw_os_pkt_complete(padapter, pndis_pkt); + +exit: + return _SUCCESS; +} + +void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue) +{ + struct list_head *plist, *phead; + struct xmit_frame *pxmitframe; + + spin_lock_bh(&(pframequeue->lock)); + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (phead != plist) { + + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + plist = get_next(plist); + + rtw_free_xmitframe(pxmitpriv, pxmitframe); + + } + spin_unlock_bh(&(pframequeue->lock)); +} + +s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue); + if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) { + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, + ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n")); +/* pxmitframe->pkt = NULL; */ + return _FAIL; + } + + return _SUCCESS; +} + +struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, sint 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_pending : BK\n")); + break; + + case 4: + case 5: + ptxservq = &(psta->sta_xmitpriv.vi_q); + *(ac) = 1; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VI\n")); + break; + + case 6: + case 7: + ptxservq = &(psta->sta_xmitpriv.vo_q); + *(ac) = 0; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VO\n")); + break; + + case 0: + case 3: + default: + ptxservq = &(psta->sta_xmitpriv.be_q); + *(ac) = 2; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BE\n")); + break; + + } + + return ptxservq; +} + +/* + * Will enqueue pxmitframe to the proper queue, + * and indicate it to xx_pending list..... + */ +s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + /* _irqL irqL0; */ + u8 ac_index; + struct sta_info *psta; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits; + sint res = _SUCCESS; + + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class); + +/* + if (pattrib->psta) { + psta = pattrib->psta; + } else { + DBG_871X("%s, call rtw_get_stainfo()\n", __func__); + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + } +*/ + + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta); + DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FAIL; + } + + if (psta == NULL) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta); + res = _FAIL; + DBG_8192C("rtw_xmit_classifier: psta == NULL\n"); + RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmit_classifier: psta == NULL\n")); + goto exit; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink); + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return _FAIL; + } + + ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index)); + + /* spin_lock_irqsave(&pstapending->lock, irqL0); */ + + if (list_empty(&ptxservq->tx_pending)) { + list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue)); + } + + /* spin_lock_irqsave(&ptxservq->sta_pending.lock, irqL1); */ + + list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending)); + ptxservq->qcnt++; + phwxmits[ac_index].accnt++; + + /* spin_unlock_irqrestore(&ptxservq->sta_pending.lock, irqL1); */ + + /* spin_unlock_irqrestore(&pstapending->lock, irqL0); */ + +exit: + + return res; +} + +void rtw_alloc_hwxmits(struct adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + pxmitpriv->hwxmit_entry = HWXMIT_ENTRY; + + pxmitpriv->hwxmits = NULL; + + pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry); + + if (pxmitpriv->hwxmits == NULL) { + DBG_871X("alloc hwxmits fail!...\n"); + return; + } + + 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_hwxmits(struct adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + hwxmits = pxmitpriv->hwxmits; + if (hwxmits) + kfree((u8 *)hwxmits); +} + +void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry) +{ + sint i; + + for (i = 0; i < entry; i++, phwxmit++) { + /* spin_lock_init(&phwxmit->xmit_lock); */ + /* INIT_LIST_HEAD(&phwxmit->pending); */ + /* phwxmit->txcmdcnt = 0; */ + phwxmit->accnt = 0; + } +} + +u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe) +{ + u32 addr; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + switch (pattrib->qsel) { + case 0: + case 3: + addr = BE_QUEUE_INX; + break; + case 1: + case 2: + addr = BK_QUEUE_INX; + break; + case 4: + case 5: + addr = VI_QUEUE_INX; + break; + case 6: + case 7: + addr = VO_QUEUE_INX; + break; + case 0x10: + addr = BCN_QUEUE_INX; + break; + case 0x11:/* BC/MC in PS (HIQ) */ + addr = HIGH_QUEUE_INX; + break; + case 0x12: + default: + addr = MGT_QUEUE_INX; + break; + + } + + return addr; + +} + +static void do_queue_select(struct adapter *padapter, struct pkt_attrib *pattrib) +{ + u8 qsel; + + qsel = pattrib->priority; + RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority =%d , qsel = %d\n", pattrib->priority, qsel)); + + pattrib->qsel = qsel; +} + +/* + * The main transmit(tx) entry + * + * Return + *1 enqueue + *0 success, hardware will handle this xmit frame(packet) + *<0 fail + */ +s32 rtw_xmit(struct adapter *padapter, _pkt **ppkt) +{ + static unsigned long start = 0; + static u32 drop_cnt = 0; + + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_frame *pxmitframe = NULL; + + s32 res; + + DBG_COUNTER(padapter->tx_logs.core_tx); + + if (start == 0) + start = jiffies; + + pxmitframe = rtw_alloc_xmitframe(pxmitpriv); + + if (jiffies_to_msecs(jiffies - start) > 2000) { + if (drop_cnt) + DBG_871X("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __func__, drop_cnt); + start = jiffies; + drop_cnt = 0; + } + + if (pxmitframe == NULL) { + drop_cnt++; + RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n")); + DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe); + return -1; + } + + res = update_attrib(padapter, *ppkt, &pxmitframe->attrib); + + if (res == _FAIL) { + RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n")); + #ifdef DBG_TX_DROP_FRAME + DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __func__); + #endif + rtw_free_xmitframe(pxmitpriv, pxmitframe); + return -1; + } + pxmitframe->pkt = *ppkt; + + do_queue_select(padapter, &pxmitframe->attrib); + + spin_lock_bh(&pxmitpriv->lock); + if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == true) { + spin_unlock_bh(&pxmitpriv->lock); + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue); + return 1; + } + spin_unlock_bh(&pxmitpriv->lock); + + /* pre_xmitframe */ + if (rtw_hal_xmit(padapter, pxmitframe) == false) + return 1; + + return 0; +} + +#define RTW_HIQ_FILTER_ALLOW_ALL 0 +#define RTW_HIQ_FILTER_ALLOW_SPECIAL 1 +#define RTW_HIQ_FILTER_DENY_ALL 2 + +inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe) +{ + bool allow = false; + struct adapter *adapter = xmitframe->padapter; + struct registry_priv *registry = &adapter->registrypriv; + + if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) { + + struct pkt_attrib *attrib = &xmitframe->attrib; + + if (attrib->ether_type == 0x0806 + || attrib->ether_type == 0x888e + || attrib->dhcp_pkt + ) { + DBG_871X(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter) + , attrib->ether_type, attrib->dhcp_pkt?" DHCP":""); + allow = true; + } + } else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL) + allow = true; + else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) { + } else + rtw_warn_on(1); + + return allow; +} + +sint xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe) +{ + sint 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; + sint bmcst = IS_MCAST(pattrib->ra); + bool update_tim = false; + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate); + return ret; + } +/* + if (pattrib->psta) + { + psta = pattrib->psta; + } + else + { + DBG_871X("%s, call rtw_get_stainfo()\n", __func__); + psta =rtw_get_stainfo(pstapriv, pattrib->ra); + } +*/ + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_sta); + DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return false; + } + + if (psta == NULL) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta); + DBG_871X("%s, psta ==NUL\n", __func__); + return false; + } + + if (!(psta->state & _FW_LINKED)) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link); + DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state); + return false; + } + + if (pattrib->triggered == 1) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger); + /* DBG_871X("directly xmit pspoll_triggered packet\n"); */ + + /* pattrib->triggered = 0; */ + if (bmcst && xmitframe_hiq_filter(pxmitframe) == true) + 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 */ + /* pattrib->qsel = 0x11;HIQ */ + + 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++; + + if (!(pstapriv->tim_bitmap & BIT(0))) + update_tim = true; + + pstapriv->tim_bitmap |= BIT(0);/* */ + pstapriv->sta_dz_bitmap |= BIT(0); + + /* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */ + + if (update_tim == true) { + update_beacon(padapter, _TIM_IE_, NULL, true); + } else { + chk_bmc_sleepq_cmd(padapter); + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + + ret = true; + + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast); + + } + + spin_unlock_bh(&psta->sleep_q.lock); + + return ret; + + } + + + spin_lock_bh(&psta->sleep_q.lock); + + if (psta->state&WIFI_SLEEP_STATE) { + u8 wmmps_ac = 0; + + if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) { + list_del_init(&pxmitframe->list); + + /* 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))) { + if (!(pstapriv->tim_bitmap & BIT(psta->aid))) + update_tim = true; + + pstapriv->tim_bitmap |= BIT(psta->aid); + + /* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */ + + if (update_tim == true) + /* DBG_871X("sleepq_len == 1, update BCNTIM\n"); */ + /* upate BCN for TIM IE */ + update_beacon(padapter, _TIM_IE_, NULL, true); + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + + /* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */ + /* */ + /* wakeup_sta_to_xmit(padapter, psta); */ + /* */ + + ret = true; + + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast); + } + + } + + spin_unlock_bh(&psta->sleep_q.lock); + + return ret; + +} + +static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue) +{ + sint ret; + struct list_head *plist, *phead; + u8 ac_index; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib; + struct xmit_frame *pxmitframe; + struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits; + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (phead != plist) { + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + plist = get_next(plist); + + pattrib = &pxmitframe->attrib; + + pattrib->triggered = 0; + + ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe); + + if (true == ret) { + ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index)); + + ptxservq->qcnt--; + phwxmits[ac_index].accnt--; + } else { + /* DBG_871X("xmitframe_enqueue_for_sleeping_sta return false\n"); */ + } + + } + +} + +void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta) +{ + struct sta_info *psta_bmc; + struct sta_xmit_priv *pstaxmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + pstaxmitpriv = &psta->sta_xmitpriv; + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo(padapter); + + + spin_lock_bh(&pxmitpriv->lock); + + psta->state |= WIFI_SLEEP_STATE; + + pstapriv->sta_dz_bitmap |= BIT(psta->aid); + + + + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending); + list_del_init(&(pstaxmitpriv->vo_q.tx_pending)); + + + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending); + list_del_init(&(pstaxmitpriv->vi_q.tx_pending)); + + + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending); + list_del_init(&(pstaxmitpriv->be_q.tx_pending)); + + + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending); + list_del_init(&(pstaxmitpriv->bk_q.tx_pending)); + + /* for BC/MC Frames */ + pstaxmitpriv = &psta_bmc->sta_xmitpriv; + dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending); + list_del_init(&(pstaxmitpriv->be_q.tx_pending)); + + spin_unlock_bh(&pxmitpriv->lock); +} + +void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta) +{ + u8 update_mask = 0, wmmps_ac = 0; + struct sta_info *psta_bmc; + struct list_head *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + psta_bmc = rtw_get_bcmc_stainfo(padapter); + + + /* spin_lock_bh(&psta->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while (xmitframe_phead != xmitframe_plist) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + list_del_init(&pxmitframe->list); + + switch (pxmitframe->attrib.priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk&BIT(1); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi&BIT(1); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo&BIT(1); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be&BIT(1); + break; + } + + psta->sleepq_len--; + if (psta->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + if (wmmps_ac) { + psta->sleepq_ac_len--; + if (psta->sleepq_ac_len > 0) { + pxmitframe->attrib.mdata = 1; + pxmitframe->attrib.eosp = 0; + } else{ + pxmitframe->attrib.mdata = 0; + pxmitframe->attrib.eosp = 1; + } + } + + pxmitframe->attrib.triggered = 1; + +/* + spin_unlock_bh(&psta->sleep_q.lock); + if (rtw_hal_xmit(padapter, pxmitframe) == true) + { + rtw_os_xmit_complete(padapter, pxmitframe); + } + spin_lock_bh(&psta->sleep_q.lock); +*/ + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + + } + + if (psta->sleepq_len == 0) { + if (pstapriv->tim_bitmap & BIT(psta->aid)) { + /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_mask = BIT(0); + } + + pstapriv->tim_bitmap &= ~BIT(psta->aid); + + if (psta->state&WIFI_SLEEP_STATE) + psta->state ^= WIFI_SLEEP_STATE; + + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) { + DBG_871X("%s alive check\n", __func__); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + + pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); + } + + /* for BC/MC Frames */ + if (!psta_bmc) + goto _exit; + + if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) { /* no any sta in ps mode */ + xmitframe_phead = get_list_head(&psta_bmc->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while (xmitframe_phead != xmitframe_plist) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + list_del_init(&pxmitframe->list); + + psta_bmc->sleepq_len--; + if (psta_bmc->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + + pxmitframe->attrib.triggered = 1; +/* + spin_unlock_bh(&psta_bmc->sleep_q.lock); + if (rtw_hal_xmit(padapter, pxmitframe) == true) + { + rtw_os_xmit_complete(padapter, pxmitframe); + } + spin_lock_bh(&psta_bmc->sleep_q.lock); + +*/ + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + } + + if (psta_bmc->sleepq_len == 0) { + if (pstapriv->tim_bitmap & BIT(0)) { + /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_mask |= BIT(1); + } + pstapriv->tim_bitmap &= ~BIT(0); + pstapriv->sta_dz_bitmap &= ~BIT(0); + } + + } + +_exit: + + /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + if (update_mask) + /* update_BCNTIM(padapter); */ + /* printk("%s => call update_beacon\n", __func__); */ + update_beacon(padapter, _TIM_IE_, NULL, true); + +} + +void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta) +{ + u8 wmmps_ac = 0; + struct list_head *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + + /* spin_lock_bh(&psta->sleep_q.lock); */ + spin_lock_bh(&pxmitpriv->lock); + + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while (xmitframe_phead != xmitframe_plist) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + 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_xmitframe_enqueue(padapter, pxmitframe); + + if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) { + pstapriv->tim_bitmap &= ~BIT(psta->aid); + + /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, true); + /* update_mask = BIT(0); */ + } + + } + + /* spin_unlock_bh(&psta->sleep_q.lock); */ + spin_unlock_bh(&pxmitpriv->lock); + + return; +} + +void enqueue_pending_xmitbuf( + struct xmit_priv *pxmitpriv, + struct xmit_buf *pxmitbuf) +{ + struct __queue *pqueue; + struct adapter *pri_adapter = pxmitpriv->adapter; + + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + spin_lock_bh(&pqueue->lock); + list_del_init(&pxmitbuf->list); + list_add_tail(&pxmitbuf->list, get_list_head(pqueue)); + spin_unlock_bh(&pqueue->lock); + + up(&(pri_adapter->xmitpriv.xmit_sema)); +} + +void enqueue_pending_xmitbuf_to_head( + struct xmit_priv *pxmitpriv, + struct xmit_buf *pxmitbuf) +{ + struct __queue *pqueue; + + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + spin_lock_bh(&pqueue->lock); + list_del_init(&pxmitbuf->list); + list_add(&pxmitbuf->list, get_list_head(pqueue)); + spin_unlock_bh(&pqueue->lock); +} + +struct xmit_buf *dequeue_pending_xmitbuf( + struct xmit_priv *pxmitpriv) +{ + struct xmit_buf *pxmitbuf; + struct __queue *pqueue; + + + pxmitbuf = NULL; + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + spin_lock_bh(&pqueue->lock); + + if (!list_empty(&pqueue->queue)) { + struct list_head *plist, *phead; + + phead = get_list_head(pqueue); + plist = get_next(phead); + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + list_del_init(&pxmitbuf->list); + } + + spin_unlock_bh(&pqueue->lock); + + return pxmitbuf; +} + +struct xmit_buf *dequeue_pending_xmitbuf_under_survey( + struct xmit_priv *pxmitpriv) +{ + struct xmit_buf *pxmitbuf; + struct __queue *pqueue; + + + pxmitbuf = NULL; + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + spin_lock_bh(&pqueue->lock); + + if (!list_empty(&pqueue->queue)) { + struct list_head *plist, *phead; + u8 type; + + phead = get_list_head(pqueue); + plist = phead; + do { + plist = get_next(plist); + if (plist == phead) + break; + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET); + + if ((type == WIFI_PROBEREQ) || + (type == WIFI_DATA_NULL) || + (type == WIFI_QOS_DATA_NULL)) { + list_del_init(&pxmitbuf->list); + break; + } + pxmitbuf = NULL; + } while (1); + } + + spin_unlock_bh(&pqueue->lock); + + return pxmitbuf; +} + +sint check_pending_xmitbuf( + struct xmit_priv *pxmitpriv) +{ + struct __queue *pqueue; + sint ret = false; + + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + spin_lock_bh(&pqueue->lock); + + if (!list_empty(&pqueue->queue)) + ret = true; + + spin_unlock_bh(&pqueue->lock); + + return ret; +} + +int rtw_xmit_thread(void *context) +{ + s32 err; + struct adapter *padapter; + + + err = _SUCCESS; + padapter = (struct adapter *)context; + + thread_enter("RTW_XMIT_THREAD"); + + do { + err = rtw_hal_xmit_thread_handler(padapter); + flush_signals_thread(); + } while (_SUCCESS == err); + + up(&padapter->xmitpriv.terminate_xmitthread_sema); + + thread_exit(); +} + +void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms) +{ + sctx->timeout_ms = timeout_ms; + sctx->submit_time = jiffies; + init_completion(&sctx->done); + sctx->status = RTW_SCTX_SUBMITTED; +} + +int rtw_sctx_wait(struct submit_ctx *sctx, const char *msg) +{ + 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_871X("%s timeout: %s\n", __func__, msg); + } else { + status = sctx->status; + } + + if (status == RTW_SCTX_DONE_SUCCESS) { + ret = _SUCCESS; + } + + return ret; +} + +static bool rtw_sctx_chk_waring_status(int status) +{ + switch (status) { + case RTW_SCTX_DONE_UNKNOWN: + case RTW_SCTX_DONE_BUF_ALLOC: + case RTW_SCTX_DONE_BUF_FREE: + + case RTW_SCTX_DONE_DRV_STOP: + case RTW_SCTX_DONE_DEV_REMOVE: + return true; + default: + return false; + } +} + +void rtw_sctx_done_err(struct submit_ctx **sctx, int status) +{ + if (*sctx) { + if (rtw_sctx_chk_waring_status(status)) + DBG_871X("%s status:%d\n", __func__, status); + (*sctx)->status = status; + complete(&((*sctx)->done)); + *sctx = NULL; + } +} + +void rtw_sctx_done(struct submit_ctx **sctx) +{ + rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS); +} + +int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + pack_tx_ops->submit_time = jiffies; + pack_tx_ops->timeout_ms = timeout_ms; + pack_tx_ops->status = RTW_SCTX_SUBMITTED; + + return rtw_sctx_wait(pack_tx_ops, __func__); +} + +void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + if (pxmitpriv->ack_tx) { + rtw_sctx_done_err(&pack_tx_ops, status); + } else { + DBG_871X("%s ack_tx not set\n", __func__); + } +} |
