static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
unsigned int len)
{
struct rtl_priv *rtlpriv = rtl_priv(hw);
struct ieee80211_mgmt *mgmt = (void *)data;
struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
u8 *pos, *end, *ie;
u16 noa_len;
static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
u8 noa_num, index, i, noa_index = 0;
bool find_p2p_ie = false , find_p2p_ps_ie = false;
pos = (u8 *)mgmt->u.beacon.variable;
end = data + len;
ie = NULL;
while (pos + 1 < end) {
if (pos + 2 + pos[1] > end)
return;
if (pos[0] == 221 && pos[1] > 4) {
if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
ie = pos + 2+4;
break;
}
}
pos += 2 + pos[1];
}
if (ie == NULL)
return;
find_p2p_ie = true;
/*to find noa ie*/
while (ie + 1 < end) {
noa_len = READEF2BYTE(&ie[1]);
if (ie + 3 + ie[1] > end)
return;
if (ie[0] == 12) {
find_p2p_ps_ie = true;
if ((noa_len - 2) % 13 != 0) {
RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
"P2P notice of absence: invalid length.%d\n",
noa_len);
return;
} else {
noa_num = (noa_len - 2) / 13;
}
noa_index = ie[3];
if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
"update NOA ie.\n");
p2pinfo->noa_index = noa_index;
p2pinfo->opp_ps = (ie[4] >> 7);
p2pinfo->ctwindow = ie[4] & 0x7F;
p2pinfo->noa_num = noa_num;
index = 5;
for (i = 0; i < noa_num; i++) {
p2pinfo->noa_count_type[i] =
READEF1BYTE(ie+index);
index += 1;
p2pinfo->noa_duration[i] =
READEF4BYTE(ie+index);
index += 4;
p2pinfo->noa_interval[i] =
READEF4BYTE(ie+index);
index += 4;
p2pinfo->noa_start_time[i] =
READEF4BYTE(ie+index);
index += 4;
}
if (p2pinfo->opp_ps == 1) {
p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
/* Driver should wait LPS entering
* CTWindow
*/
if (rtlpriv->psc.fw_current_inpsmode)
rtl_p2p_ps_cmd(hw,
P2P_PS_ENABLE);
} else if (p2pinfo->noa_num > 0) {
p2pinfo->p2p_ps_mode = P2P_PS_NOA;
rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
}
}
break;
}
static void rtl_op_bss_info_changed( struct ieee80211_hw *hw,
struct ieee80211_vif *vif,
struct ieee80211_bss_conf *bss_conf, u32 changed )
{
struct rtl_priv *rtlpriv = rtl_priv( hw );
struct rtl_hal *rtlhal = rtl_hal( rtlpriv );
struct rtl_mac *mac = rtl_mac( rtl_priv( hw ) );
struct rtl_ps_ctl *ppsc = rtl_psc( rtl_priv( hw ) );
struct ieee80211_sta *sta = NULL;
mutex_lock( &rtlpriv->locks.conf_mutex );
if ( ( vif->type == NL80211_IFTYPE_ADHOC ) ||
( vif->type == NL80211_IFTYPE_AP ) ||
( vif->type == NL80211_IFTYPE_MESH_POINT ) ) {
if ( ( changed & BSS_CHANGED_BEACON ) ||
( changed & BSS_CHANGED_BEACON_ENABLED &&
bss_conf->enable_beacon ) ) {
if ( mac->beacon_enabled == 0 ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_DMESG,
"BSS_CHANGED_BEACON_ENABLED\n" );
/*start hw beacon interrupt. */
/*rtlpriv->cfg->ops->set_bcn_reg( hw ); */
mac->beacon_enabled = 1;
rtlpriv->cfg->ops->update_interrupt_mask( hw,
rtlpriv->cfg->maps
[RTL_IBSS_INT_MASKS],
0 );
if ( rtlpriv->cfg->ops->linked_set_reg )
rtlpriv->cfg->ops->linked_set_reg( hw );
}
}
if ( ( changed & BSS_CHANGED_BEACON_ENABLED &&
!bss_conf->enable_beacon ) ) {
if ( mac->beacon_enabled == 1 ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_DMESG,
"ADHOC DISABLE BEACON\n" );
mac->beacon_enabled = 0;
rtlpriv->cfg->ops->update_interrupt_mask( hw, 0,
rtlpriv->cfg->maps
[RTL_IBSS_INT_MASKS] );
}
}
if ( changed & BSS_CHANGED_BEACON_INT ) {
RT_TRACE( rtlpriv, COMP_BEACON, DBG_TRACE,
"BSS_CHANGED_BEACON_INT\n" );
mac->beacon_interval = bss_conf->beacon_int;
rtlpriv->cfg->ops->set_bcn_intv( hw );
}
}
/*TODO: reference to enum ieee80211_bss_change */
if ( changed & BSS_CHANGED_ASSOC ) {
if ( bss_conf->assoc ) {
struct ieee80211_sta *sta = NULL;
/* we should reset all sec info & cam
* before set cam after linked, we should not
* reset in disassoc, that will cause tkip->wep
* fail because some flag will be wrong */
/* reset sec info */
rtl_cam_reset_sec_info( hw );
/* reset cam to fix wep fail issue
* when change from wpa to wep */
rtl_cam_reset_all_entry( hw );
mac->link_state = MAC80211_LINKED;
mac->cnt_after_linked = 0;
mac->assoc_id = bss_conf->aid;
memcpy( mac->bssid, bss_conf->bssid, ETH_ALEN );
if ( rtlpriv->cfg->ops->linked_set_reg )
rtlpriv->cfg->ops->linked_set_reg( hw );
rcu_read_lock();
sta = ieee80211_find_sta( vif, ( u8 * )bss_conf->bssid );
if ( !sta ) {
pr_err( "ieee80211_find_sta returned NULL\n" );
rcu_read_unlock();
goto out;
}
if ( vif->type == NL80211_IFTYPE_STATION && sta )
rtlpriv->cfg->ops->update_rate_tbl( hw, sta, 0 );
RT_TRACE( rtlpriv, COMP_EASY_CONCURRENT, DBG_LOUD,
"send PS STATIC frame\n" );
if ( rtlpriv->dm.supp_phymode_switch ) {
if ( sta->ht_cap.ht_supported )
rtl_send_smps_action( hw, sta,
IEEE80211_SMPS_STATIC );
}
rcu_read_unlock();
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_DMESG,
"BSS_CHANGED_ASSOC\n" );
} else {
if ( mac->link_state == MAC80211_LINKED ) {
rtlpriv->enter_ps = false;
schedule_work( &rtlpriv->works.lps_change_work );
}
if ( ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE )
rtl_p2p_ps_cmd( hw, P2P_PS_DISABLE );
mac->link_state = MAC80211_NOLINK;
memset( mac->bssid, 0, ETH_ALEN );
mac->vendor = PEER_UNKNOWN;
if ( rtlpriv->dm.supp_phymode_switch ) {
if ( rtlpriv->cfg->ops->chk_switch_dmdp )
rtlpriv->cfg->ops->chk_switch_dmdp( hw );
}
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_DMESG,
"BSS_CHANGED_UN_ASSOC\n" );
}
}
if ( changed & BSS_CHANGED_ERP_CTS_PROT ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_TRACE,
"BSS_CHANGED_ERP_CTS_PROT\n" );
mac->use_cts_protect = bss_conf->use_cts_prot;
}
if ( changed & BSS_CHANGED_ERP_PREAMBLE ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_LOUD,
"BSS_CHANGED_ERP_PREAMBLE use short preamble:%x\n",
bss_conf->use_short_preamble );
mac->short_preamble = bss_conf->use_short_preamble;
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_ACK_PREAMBLE,
&mac->short_preamble );
}
if ( changed & BSS_CHANGED_ERP_SLOT ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_TRACE,
"BSS_CHANGED_ERP_SLOT\n" );
if ( bss_conf->use_short_slot )
mac->slot_time = RTL_SLOT_TIME_9;
else
mac->slot_time = RTL_SLOT_TIME_20;
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_SLOT_TIME,
&mac->slot_time );
}
if ( changed & BSS_CHANGED_HT ) {
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_TRACE, "BSS_CHANGED_HT\n" );
rcu_read_lock();
sta = get_sta( hw, vif, bss_conf->bssid );
if ( sta ) {
if ( sta->ht_cap.ampdu_density >
mac->current_ampdu_density )
mac->current_ampdu_density =
sta->ht_cap.ampdu_density;
if ( sta->ht_cap.ampdu_factor <
mac->current_ampdu_factor )
mac->current_ampdu_factor =
sta->ht_cap.ampdu_factor;
}
rcu_read_unlock();
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_SHORTGI_DENSITY,
&mac->max_mss_density );
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_AMPDU_FACTOR,
&mac->current_ampdu_factor );
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_AMPDU_MIN_SPACE,
&mac->current_ampdu_density );
}
if ( changed & BSS_CHANGED_BSSID ) {
u32 basic_rates;
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_BSSID,
( u8 * ) bss_conf->bssid );
RT_TRACE( rtlpriv, COMP_MAC80211, DBG_DMESG, "%pM\n",
bss_conf->bssid );
mac->vendor = PEER_UNKNOWN;
memcpy( mac->bssid, bss_conf->bssid, ETH_ALEN );
rtlpriv->cfg->ops->set_network_type( hw, vif->type );
rcu_read_lock();
sta = get_sta( hw, vif, bss_conf->bssid );
if ( !sta ) {
rcu_read_unlock();
goto out;
}
if ( rtlhal->current_bandtype == BAND_ON_5G ) {
mac->mode = WIRELESS_MODE_A;
} else {
if ( sta->supp_rates[0] <= 0xf )
mac->mode = WIRELESS_MODE_B;
else
mac->mode = WIRELESS_MODE_G;
}
if ( sta->ht_cap.ht_supported ) {
if ( rtlhal->current_bandtype == BAND_ON_2_4G )
mac->mode = WIRELESS_MODE_N_24G;
else
mac->mode = WIRELESS_MODE_N_5G;
}
/* just station need it, because ibss & ap mode will
* set in sta_add, and will be NULL here */
if ( mac->opmode == NL80211_IFTYPE_STATION ) {
struct rtl_sta_info *sta_entry;
sta_entry = ( struct rtl_sta_info * ) sta->drv_priv;
sta_entry->wireless_mode = mac->mode;
}
if ( sta->ht_cap.ht_supported ) {
mac->ht_enable = true;
/*
* for cisco 1252 bw20 it's wrong
* if ( ht_cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40 ) {
* mac->bw_40 = true;
* }
* */
}
if ( changed & BSS_CHANGED_BASIC_RATES ) {
/* for 5G must << RATE_6M_INDEX = 4,
* because 5G have no cck rate*/
if ( rtlhal->current_bandtype == BAND_ON_5G )
basic_rates = sta->supp_rates[1] << 4;
else
basic_rates = sta->supp_rates[0];
mac->basic_rates = basic_rates;
rtlpriv->cfg->ops->set_hw_reg( hw, HW_VAR_BASIC_RATE,
( u8 * )( &basic_rates ) );
}
rcu_read_unlock();
}
/*
* For FW LPS:
* To tell firmware we have connected
* to an AP. For 92SE/CE power save v2.
*/
if ( changed & BSS_CHANGED_ASSOC ) {
if ( bss_conf->assoc ) {
if ( ppsc->fwctrl_lps ) {
u8 mstatus = RT_MEDIA_CONNECT;
u8 keep_alive = 10;
rtlpriv->cfg->ops->set_hw_reg( hw,
HW_VAR_KEEP_ALIVE,
&keep_alive );
rtlpriv->cfg->ops->set_hw_reg( hw,
HW_VAR_H2C_FW_JOINBSSRPT,
&mstatus );
ppsc->report_linked = true;
}
} else {
if ( ppsc->fwctrl_lps ) {
u8 mstatus = RT_MEDIA_DISCONNECT;
rtlpriv->cfg->ops->set_hw_reg( hw,
HW_VAR_H2C_FW_JOINBSSRPT,
&mstatus );
ppsc->report_linked = false;
}
}
if ( rtlpriv->cfg->ops->bt_wifi_media_status_notify )
rtlpriv->cfg->ops->bt_wifi_media_status_notify( hw,
ppsc->report_linked );
}
out:
mutex_unlock( &rtlpriv->locks.conf_mutex );
}
/* Change current and default preamble mode.*/
static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
{
struct rtl_priv *rtlpriv = rtl_priv(hw);
struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
bool enter_fwlps;
if (mac->opmode == NL80211_IFTYPE_ADHOC)
return;
if (mac->link_state != MAC80211_LINKED)
return;
if (ppsc->dot11_psmode == rt_psmode)
return;
/* Update power save mode configured. */
ppsc->dot11_psmode = rt_psmode;
/*
*<FW control LPS>
*1. Enter PS mode
* Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
* cmd to set Fw into PS mode.
*2. Leave PS mode
* Send H2C fw_pwrmode cmd to Fw to set Fw into Active
* mode and set RPWM to turn RF on.
*/
if ((ppsc->fwctrl_lps) && ppsc->report_linked) {
if (ppsc->dot11_psmode == EACTIVE) {
RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
"FW LPS leave ps_mode:%x\n",
FW_PS_ACTIVE_MODE);
enter_fwlps = false;
ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
ppsc->smart_ps = 0;
rtlpriv->cfg->ops->set_hw_reg(hw,
HW_VAR_FW_LPS_ACTION,
(u8 *)(&enter_fwlps));
if (ppsc->p2p_ps_info.opp_ps)
rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
} else {
if (rtl_get_fwlps_doze(hw)) {
RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
"FW LPS enter ps_mode:%x\n",
ppsc->fwctrl_psmode);
enter_fwlps = true;
ppsc->pwr_mode = ppsc->fwctrl_psmode;
ppsc->smart_ps = 2;
rtlpriv->cfg->ops->set_hw_reg(hw,
HW_VAR_FW_LPS_ACTION,
(u8 *)(&enter_fwlps));
} else {
/* Reset the power save related parameters. */
ppsc->dot11_psmode = EACTIVE;
}
}
}
}
