[deliverable/linux.git] / drivers / net / wireless / rtlwifi / ps.c

/******************************************************************************
 *
 * Copyright(c) 2009-2012  Realtek Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
 *
 * The full GNU General Public License is included in this distribution in the
 * file called LICENSE.
 *
 * Contact Information:
 * wlanfae <wlanfae@realtek.com>
 * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
 * Hsinchu 300, Taiwan.
 *
 * Larry Finger <Larry.Finger@lwfinger.net>
 *
 *****************************************************************************/

#include <linux/export.h>
#include "wifi.h"
#include "base.h"
#include "ps.h"

bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));

	/*<1> reset trx ring */
	if (rtlhal->interface == INTF_PCI)
		rtlpriv->intf_ops->reset_trx_ring(hw);

	if (is_hal_stop(rtlhal))
		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
			 "Driver is already down!\n");

	/*<2> Enable Adapter */
	if (rtlpriv->cfg->ops->hw_init(hw))
		return 1;
	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);

	/*<3> Enable Interrupt */
	rtlpriv->cfg->ops->enable_interrupt(hw);

	/*<enable timer> */
	rtl_watch_dog_timer_callback((unsigned long)hw);

	return true;
}
EXPORT_SYMBOL(rtl_ps_enable_nic);

bool rtl_ps_disable_nic(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);

	/*<1> Stop all timer */
	rtl_deinit_deferred_work(hw);

	/*<2> Disable Interrupt */
	rtlpriv->cfg->ops->disable_interrupt(hw);
	tasklet_kill(&rtlpriv->works.irq_tasklet);

	/*<3> Disable Adapter */
	rtlpriv->cfg->ops->hw_disable(hw);

	return true;
}
EXPORT_SYMBOL(rtl_ps_disable_nic);

bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
			 enum rf_pwrstate state_toset,
			 u32 changesource)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	bool actionallowed = false;

	switch (state_toset) {
	case ERFON:
		ppsc->rfoff_reason &= (~changesource);

		if ((changesource == RF_CHANGE_BY_HW) &&
		    (ppsc->hwradiooff)) {
			ppsc->hwradiooff = false;
		}

		if (!ppsc->rfoff_reason) {
			ppsc->rfoff_reason = 0;
			actionallowed = true;
		}

		break;

	case ERFOFF:

		if ((changesource == RF_CHANGE_BY_HW)
		    && (ppsc->hwradiooff == false)) {
			ppsc->hwradiooff = true;
		}

		ppsc->rfoff_reason |= changesource;
		actionallowed = true;
		break;

	case ERFSLEEP:
		ppsc->rfoff_reason |= changesource;
		actionallowed = true;
		break;

	default:
		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
			 "switch case not processed\n");
		break;
	}

	if (actionallowed)
		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);

	return actionallowed;
}
EXPORT_SYMBOL(rtl_ps_set_rf_state);

static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));

	ppsc->swrf_processing = true;

	if (ppsc->inactive_pwrstate == ERFON &&
	    rtlhal->interface == INTF_PCI) {
		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
		    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
		    rtlhal->interface == INTF_PCI) {
			rtlpriv->intf_ops->disable_aspm(hw);
			RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
		}
	}

	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, RF_CHANGE_BY_IPS);

	if (ppsc->inactive_pwrstate == ERFOFF &&
	    rtlhal->interface == INTF_PCI) {
		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
			!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
			rtlpriv->intf_ops->enable_aspm(hw);
			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
		}
	}

	ppsc->swrf_processing = false;
}

void rtl_ips_nic_off_wq_callback(void *data)
{
	struct rtl_works *rtlworks =
	    container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
	struct ieee80211_hw *hw = rtlworks->hw;
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	enum rf_pwrstate rtstate;

	if (mac->opmode != NL80211_IFTYPE_STATION) {
		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
			 "not station return\n");
		return;
	}

	if (mac->link_state > MAC80211_NOLINK)
		return;

	if (is_hal_stop(rtlhal))
		return;

	if (rtlpriv->sec.being_setkey)
		return;

	if (ppsc->inactiveps) {
		rtstate = ppsc->rfpwr_state;

		/*
		 *Do not enter IPS in the following conditions:
		 *(1) RF is already OFF or Sleep
		 *(2) swrf_processing (indicates the IPS is still under going)
		 *(3) Connectted (only disconnected can trigger IPS)
		 *(4) IBSS (send Beacon)
		 *(5) AP mode (send Beacon)
		 *(6) monitor mode (rcv packet)
		 */

		if (rtstate == ERFON &&
		    !ppsc->swrf_processing &&
		    (mac->link_state == MAC80211_NOLINK) &&
		    !mac->act_scanning) {
			RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
				 "IPSEnter(): Turn off RF\n");

			ppsc->inactive_pwrstate = ERFOFF;
			ppsc->in_powersavemode = true;

			/*rtl_pci_reset_trx_ring(hw); */
			_rtl_ps_inactive_ps(hw);
		}
	}
}

void rtl_ips_nic_off(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);

	/*
	 *because when link with ap, mac80211 will ask us
	 *to disable nic quickly after scan before linking,
	 *this will cause link failed, so we delay 100ms here
	 */
	queue_delayed_work(rtlpriv->works.rtl_wq,
			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
}

void rtl_ips_nic_on(struct ieee80211_hw *hw)
{
	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));
	enum rf_pwrstate rtstate;
	unsigned long flags;

	if (mac->opmode != NL80211_IFTYPE_STATION)
		return;

	spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);

	if (ppsc->inactiveps) {
		rtstate = ppsc->rfpwr_state;

		if (rtstate != ERFON &&
		    !ppsc->swrf_processing &&
		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {

			ppsc->inactive_pwrstate = ERFON;
			ppsc->in_powersavemode = false;

			_rtl_ps_inactive_ps(hw);
		}
	}

	spin_unlock_irqrestore(&rtlpriv->locks.ips_lock, flags);
}

/*for FW LPS*/

/*
 *Determine if we can set Fw into PS mode
 *in current condition.Return TRUE if it
 *can enter PS mode.
 */
static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
{
	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));
	u32 ps_timediff;

	ps_timediff = jiffies_to_msecs(jiffies -
				       ppsc->last_delaylps_stamp_jiffies);

	if (ps_timediff < 2000) {
		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
			 "Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n");
		return false;
	}

	if (mac->link_state != MAC80211_LINKED)
		return false;

	if (mac->opmode == NL80211_IFTYPE_ADHOC)
		return false;

	return true;
}

/* 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));
	u8 rpwm_val, fw_pwrmode;

	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) {
		bool fw_current_inps;
		if (ppsc->dot11_psmode == EACTIVE) {
			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
				 "FW LPS leave ps_mode:%x\n",
				 FW_PS_ACTIVE_MODE);

			rpwm_val = 0x0C;	/* RF on */
			fw_pwrmode = FW_PS_ACTIVE_MODE;
			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
					(u8 *) (&rpwm_val));
			rtlpriv->cfg->ops->set_hw_reg(hw,
					HW_VAR_H2C_FW_PWRMODE,
					(u8 *) (&fw_pwrmode));
			fw_current_inps = false;

			rtlpriv->cfg->ops->set_hw_reg(hw,
					HW_VAR_FW_PSMODE_STATUS,
					(u8 *) (&fw_current_inps));

		} else {
			if (rtl_get_fwlps_doze(hw)) {
				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
					 "FW LPS enter ps_mode:%x\n",
					 ppsc->fwctrl_psmode);

				rpwm_val = 0x02;	/* RF off */
				fw_current_inps = true;
				rtlpriv->cfg->ops->set_hw_reg(hw,
						HW_VAR_FW_PSMODE_STATUS,
						(u8 *) (&fw_current_inps));
				rtlpriv->cfg->ops->set_hw_reg(hw,
						HW_VAR_H2C_FW_PWRMODE,
						(u8 *) (&ppsc->fwctrl_psmode));

				rtlpriv->cfg->ops->set_hw_reg(hw,
						HW_VAR_SET_RPWM,
						(u8 *) (&rpwm_val));
			} else {
				/* Reset the power save related parameters. */
				ppsc->dot11_psmode = EACTIVE;
			}
		}
	}
}

/*Enter the leisure power save mode.*/
void rtl_lps_enter(struct ieee80211_hw *hw)
{
	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	struct rtl_priv *rtlpriv = rtl_priv(hw);

	if (!ppsc->fwctrl_lps)
		return;

	if (rtlpriv->sec.being_setkey)
		return;

	if (rtlpriv->link_info.busytraffic)
		return;

	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
	if (mac->cnt_after_linked < 5)
		return;

	if (mac->opmode == NL80211_IFTYPE_ADHOC)
		return;

	if (mac->link_state != MAC80211_LINKED)
		return;

	mutex_lock(&rtlpriv->locks.ps_mutex);

	/* Idle for a while if we connect to AP a while ago. */
	if (mac->cnt_after_linked >= 2) {
		if (ppsc->dot11_psmode == EACTIVE) {
			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
				 "Enter 802.11 power save mode...\n");

			rtl_lps_set_psmode(hw, EAUTOPS);
		}
	}

	mutex_unlock(&rtlpriv->locks.ps_mutex);
}

/*Leave the leisure power save mode.*/
void rtl_lps_leave(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));

	mutex_lock(&rtlpriv->locks.ps_mutex);

	if (ppsc->fwctrl_lps) {
		if (ppsc->dot11_psmode != EACTIVE) {

			/*FIX ME */
			rtlpriv->cfg->ops->enable_interrupt(hw);

			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
			    rtlhal->interface == INTF_PCI) {
				rtlpriv->intf_ops->disable_aspm(hw);
				RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
			}

			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
				 "Busy Traffic,Leave 802.11 power save..\n");

			rtl_lps_set_psmode(hw, EACTIVE);
		}
	}
	mutex_unlock(&rtlpriv->locks.ps_mutex);
}

/* For sw LPS*/
void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	struct ieee80211_hdr *hdr = (void *) data;
	struct ieee80211_tim_ie *tim_ie;
	u8 *tim;
	u8 tim_len;
	bool u_buffed;
	bool m_buffed;

	if (mac->opmode != NL80211_IFTYPE_STATION)
		return;

	if (!rtlpriv->psc.swctrl_lps)
		return;

	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
		return;

	if (!rtlpriv->psc.sw_ps_enabled)
		return;

	if (rtlpriv->psc.fwctrl_lps)
		return;

	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
		return;

	/* check if this really is a beacon */
	if (!ieee80211_is_beacon(hdr->frame_control))
		return;

	/* min. beacon length + FCS_LEN */
	if (len <= 40 + FCS_LEN)
		return;

	/* and only beacons from the associated BSSID, please */
	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
		return;

	rtlpriv->psc.last_beacon = jiffies;

	tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
	if (!tim)
		return;

	if (tim[1] < sizeof(*tim_ie))
		return;

	tim_len = tim[1];
	tim_ie = (struct ieee80211_tim_ie *) &tim[2];

	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;

	/* Check whenever the PHY can be turned off again. */

	/* 1. What about buffered unicast traffic for our AID? */
	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
				       rtlpriv->mac80211.assoc_id);

	/* 2. Maybe the AP wants to send multicast/broadcast data? */
	m_buffed = tim_ie->bitmap_ctrl & 0x01;
	rtlpriv->psc.multi_buffered = m_buffed;

	/* unicast will process by mac80211 through
	 * set ~IEEE80211_CONF_PS, So we just check
	 * multicast frames here */
	if (!m_buffed) {
		/* back to low-power land. and delay is
		 * prevent null power save frame tx fail */
		queue_delayed_work(rtlpriv->works.rtl_wq,
				&rtlpriv->works.ps_work, MSECS(5));
	} else {
		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
			 "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
	}
}

void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
{
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));

	if (!rtlpriv->psc.swctrl_lps)
		return;
	if (mac->link_state != MAC80211_LINKED)
		return;

	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
		RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
		rtlpriv->intf_ops->disable_aspm(hw);
		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
	}

	mutex_lock(&rtlpriv->locks.ps_mutex);
	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
	mutex_unlock(&rtlpriv->locks.ps_mutex);
}

void rtl_swlps_rfon_wq_callback(void *data)
{
	struct rtl_works *rtlworks =
	    container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
	struct ieee80211_hw *hw = rtlworks->hw;

	rtl_swlps_rf_awake(hw);
}

void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
{
	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));
	u8 sleep_intv;

	if (!rtlpriv->psc.sw_ps_enabled)
		return;

	if ((rtlpriv->sec.being_setkey) ||
	    (mac->opmode == NL80211_IFTYPE_ADHOC))
		return;

	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
	if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
		return;

	if (rtlpriv->link_info.busytraffic)
		return;

	mutex_lock(&rtlpriv->locks.ps_mutex);
	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
	mutex_unlock(&rtlpriv->locks.ps_mutex);

	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
		!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
		rtlpriv->intf_ops->enable_aspm(hw);
		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
	}

	/* here is power save alg, when this beacon is DTIM
	 * we will set sleep time to dtim_period * n;
	 * when this beacon is not DTIM, we will set sleep
	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */

	if (rtlpriv->psc.dtim_counter == 0) {
		if (hw->conf.ps_dtim_period == 1)
			sleep_intv = hw->conf.ps_dtim_period * 2;
		else
			sleep_intv = hw->conf.ps_dtim_period;
	} else {
		sleep_intv = rtlpriv->psc.dtim_counter;
	}

	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
		sleep_intv = MAX_SW_LPS_SLEEP_INTV;

	/* this print should always be dtim_conter = 0 &
	 * sleep  = dtim_period, that meaons, we should
	 * awake before every dtim */
	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
		 "dtim_counter:%x will sleep :%d beacon_intv\n",
		 rtlpriv->psc.dtim_counter, sleep_intv);

	/* we tested that 40ms is enough for sw & hw sw delay */
	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
			MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
}


void rtl_swlps_wq_callback(void *data)
{
	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
				     struct rtl_works,
				     ps_work);
	struct ieee80211_hw *hw = rtlworks->hw;
	struct rtl_priv *rtlpriv = rtl_priv(hw);
	bool ps = false;

	ps = (hw->conf.flags & IEEE80211_CONF_PS);

	/* we can sleep after ps null send ok */
	if (rtlpriv->psc.state_inap) {
		rtl_swlps_rf_sleep(hw);

		if (rtlpriv->psc.state && !ps) {
			rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
					rtlpriv->psc.last_action);
		}

		if (ps)
			rtlpriv->psc.last_slept = jiffies;

		rtlpriv->psc.last_action = jiffies;
		rtlpriv->psc.state = ps;
	}
}
Commit	Line	Data
0c817338 LF	1	/******************************************************************************
0c817338 LF	2	*
a8d76066	3	* Copyright(c) 2009-2012 Realtek Corporation.
0c817338 LF	4	*
	5	* This program is free software; you can redistribute it and/or modify it
	6	* under the terms of version 2 of the GNU General Public License as
	7	* published by the Free Software Foundation.
	8	*
	9	* This program is distributed in the hope that it will be useful, but WITHOUT
	10	* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
	11	* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
	12	* more details.
	13	*
	14	* You should have received a copy of the GNU General Public License along with
	15	* this program; if not, write to the Free Software Foundation, Inc.,
	16	* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
	17	*
	18	* The full GNU General Public License is included in this distribution in the
	19	* file called LICENSE.
	20	*
	21	* Contact Information:
	22	* wlanfae <wlanfae@realtek.com>
	23	* Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
	24	* Hsinchu 300, Taiwan.
	25	*
	26	* Larry Finger <Larry.Finger@lwfinger.net>
	27	*
	28	*****************************************************************************/
	29
ee40fa06	30	#include <linux/export.h>
0c817338 LF	31	#include "wifi.h"
	32	#include "base.h"
	33	#include "ps.h"
	34
	35	bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
	36	{
	37	struct rtl_priv *rtlpriv = rtl_priv(hw);
	38	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	39	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
0c817338 LF	40
	41	/<1> reset trx ring /
	42	if (rtlhal->interface == INTF_PCI)
	43	rtlpriv->intf_ops->reset_trx_ring(hw);
	44
	45	if (is_hal_stop(rtlhal))
	46	RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
f30d7507	47	"Driver is already down!\n");
0c817338 LF	48
0c817338 LF	49	/<2> Enable Adapter /
b0302aba LF	50	if (rtlpriv->cfg->ops->hw_init(hw))
b0302aba LF	51	return 1;
0c817338	52	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
0c817338 LF	53
	54	/<3> Enable Interrupt /
	55	rtlpriv->cfg->ops->enable_interrupt(hw);
	56
	57	/<enable timer> /
	58	rtl_watch_dog_timer_callback((unsigned long)hw);
	59
cc7dc0c4	60	return true;
0c817338 LF	61	}
	62	EXPORT_SYMBOL(rtl_ps_enable_nic);
	63
	64	bool rtl_ps_disable_nic(struct ieee80211_hw *hw)
	65	{
0c817338 LF	66	struct rtl_priv *rtlpriv = rtl_priv(hw);
	67
	68	/<1> Stop all timer /
	69	rtl_deinit_deferred_work(hw);
	70
	71	/<2> Disable Interrupt /
	72	rtlpriv->cfg->ops->disable_interrupt(hw);
67fc6052	73	tasklet_kill(&rtlpriv->works.irq_tasklet);
0c817338 LF	74
	75	/<3> Disable Adapter /
	76	rtlpriv->cfg->ops->hw_disable(hw);
	77
32473284	78	return true;
0c817338 LF	79	}
	80	EXPORT_SYMBOL(rtl_ps_disable_nic);
	81
	82	bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
	83	enum rf_pwrstate state_toset,
4b9d8d67	84	u32 changesource)
0c817338 LF	85	{
	86	struct rtl_priv *rtlpriv = rtl_priv(hw);
	87	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
7ea47240	88	bool actionallowed = false;
0c817338	89
0c817338 LF	90	switch (state_toset) {
	91	case ERFON:
	92	ppsc->rfoff_reason &= (~changesource);
	93
	94	if ((changesource == RF_CHANGE_BY_HW) &&
e10542c4	95	(ppsc->hwradiooff)) {
7ea47240	96	ppsc->hwradiooff = false;
0c817338 LF	97	}
	98
	99	if (!ppsc->rfoff_reason) {
	100	ppsc->rfoff_reason = 0;
7ea47240	101	actionallowed = true;
0c817338 LF	102	}
	103
	104	break;
	105
	106	case ERFOFF:
	107
	108	if ((changesource == RF_CHANGE_BY_HW)
7ea47240 LF	109	&& (ppsc->hwradiooff == false)) {
7ea47240 LF	110	ppsc->hwradiooff = true;
0c817338 LF	111	}
	112
	113	ppsc->rfoff_reason \|= changesource;
7ea47240	114	actionallowed = true;
0c817338 LF	115	break;
	116
	117	case ERFSLEEP:
	118	ppsc->rfoff_reason \|= changesource;
7ea47240	119	actionallowed = true;
0c817338 LF	120	break;
	121
	122	default:
	123	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
f30d7507	124	"switch case not processed\n");
0c817338 LF	125	break;
	126	}
	127
7ea47240	128	if (actionallowed)
0c817338 LF	129	rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
0c817338 LF	130
7ea47240	131	return actionallowed;
0c817338 LF	132	}
	133	EXPORT_SYMBOL(rtl_ps_set_rf_state);
	134
	135	static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
	136	{
	137	struct rtl_priv *rtlpriv = rtl_priv(hw);
	138	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
	139	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	140
7ea47240	141	ppsc->swrf_processing = true;
0c817338	142
099fb8ab	143	if (ppsc->inactive_pwrstate == ERFON &&
cc7dc0c4	144	rtlhal->interface == INTF_PCI) {
0c817338	145	if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
cc7dc0c4	146	RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
0c817338 LF	147	rtlhal->interface == INTF_PCI) {
0c817338 LF	148	rtlpriv->intf_ops->disable_aspm(hw);
cc7dc0c4	149	RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
0c817338 LF	150	}
	151	}
	152
4b9d8d67	153	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, RF_CHANGE_BY_IPS);
0c817338 LF	154
	155	if (ppsc->inactive_pwrstate == ERFOFF &&
	156	rtlhal->interface == INTF_PCI) {
cc7dc0c4 C	157	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
cc7dc0c4 C	158	!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
0c817338	159	rtlpriv->intf_ops->enable_aspm(hw);
cc7dc0c4	160	RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
0c817338 LF	161	}
	162	}
	163
7ea47240	164	ppsc->swrf_processing = false;
0c817338 LF	165	}
	166
	167	void rtl_ips_nic_off_wq_callback(void *data)
	168	{
	169	struct rtl_works *rtlworks =
	170	container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
	171	struct ieee80211_hw *hw = rtlworks->hw;
	172	struct rtl_priv *rtlpriv = rtl_priv(hw);
	173	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
	174	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	175	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	176	enum rf_pwrstate rtstate;
	177
	178	if (mac->opmode != NL80211_IFTYPE_STATION) {
	179	RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
f30d7507	180	"not station return\n");
0c817338 LF	181	return;
	182	}
	183
cc7dc0c4 C	184	if (mac->link_state > MAC80211_NOLINK)
	185	return;
	186
0c817338 LF	187	if (is_hal_stop(rtlhal))
	188	return;
	189
	190	if (rtlpriv->sec.being_setkey)
	191	return;
	192
7ea47240	193	if (ppsc->inactiveps) {
0c817338 LF	194	rtstate = ppsc->rfpwr_state;
	195
	196	/*
	197	*Do not enter IPS in the following conditions:
	198	*(1) RF is already OFF or Sleep
7ea47240	199	*(2) swrf_processing (indicates the IPS is still under going)
0c817338 LF	200	*(3) Connectted (only disconnected can trigger IPS)
	201	*(4) IBSS (send Beacon)
	202	*(5) AP mode (send Beacon)
	203	*(6) monitor mode (rcv packet)
	204	*/
	205
	206	if (rtstate == ERFON &&
7ea47240	207	!ppsc->swrf_processing &&
0c817338 LF	208	(mac->link_state == MAC80211_NOLINK) &&
	209	!mac->act_scanning) {
	210	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
f30d7507	211	"IPSEnter(): Turn off RF\n");
0c817338 LF	212
0c817338 LF	213	ppsc->inactive_pwrstate = ERFOFF;
7ea47240	214	ppsc->in_powersavemode = true;
0c817338 LF	215
	216	/rtl_pci_reset_trx_ring(hw); /
	217	_rtl_ps_inactive_ps(hw);
	218	}
	219	}
	220	}
	221
	222	void rtl_ips_nic_off(struct ieee80211_hw *hw)
	223	{
	224	struct rtl_priv *rtlpriv = rtl_priv(hw);
	225
	226	/*
	227	*because when link with ap, mac80211 will ask us
	228	*to disable nic quickly after scan before linking,
	229	*this will cause link failed, so we delay 100ms here
	230	*/
	231	queue_delayed_work(rtlpriv->works.rtl_wq,
	232	&rtlpriv->works.ips_nic_off_wq, MSECS(100));
	233	}
	234
	235	void rtl_ips_nic_on(struct ieee80211_hw *hw)
	236	{
	237	struct rtl_priv *rtlpriv = rtl_priv(hw);
cc7dc0c4	238	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
0c817338 LF	239	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
0c817338 LF	240	enum rf_pwrstate rtstate;
b9116b9a	241	unsigned long flags;
0c817338	242
cc7dc0c4 C	243	if (mac->opmode != NL80211_IFTYPE_STATION)
	244	return;
	245
b9116b9a	246	spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);
0c817338	247
7ea47240	248	if (ppsc->inactiveps) {
0c817338 LF	249	rtstate = ppsc->rfpwr_state;
	250
	251	if (rtstate != ERFON &&
7ea47240	252	!ppsc->swrf_processing &&
0c817338 LF	253	ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
	254
	255	ppsc->inactive_pwrstate = ERFON;
7ea47240	256	ppsc->in_powersavemode = false;
0c817338 LF	257
	258	_rtl_ps_inactive_ps(hw);
	259	}
	260	}
	261
b9116b9a	262	spin_unlock_irqrestore(&rtlpriv->locks.ips_lock, flags);
0c817338 LF	263	}
	264
	265	/for FW LPS/
	266
	267	/*
	268	*Determine if we can set Fw into PS mode
	269	*in current condition.Return TRUE if it
	270	*can enter PS mode.
	271	*/
	272	static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
	273	{
	274	struct rtl_priv *rtlpriv = rtl_priv(hw);
	275	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	276	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	277	u32 ps_timediff;
	278
	279	ps_timediff = jiffies_to_msecs(jiffies -
	280	ppsc->last_delaylps_stamp_jiffies);
	281
	282	if (ps_timediff < 2000) {
	283	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
f30d7507	284	"Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n");
0c817338 LF	285	return false;
	286	}
	287
	288	if (mac->link_state != MAC80211_LINKED)
	289	return false;
	290
	291	if (mac->opmode == NL80211_IFTYPE_ADHOC)
	292	return false;
	293
	294	return true;
	295	}
	296
	297	/* Change current and default preamble mode.*/
	298	static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
	299	{
	300	struct rtl_priv *rtlpriv = rtl_priv(hw);
	301	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	302	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	303	u8 rpwm_val, fw_pwrmode;
	304
	305	if (mac->opmode == NL80211_IFTYPE_ADHOC)
	306	return;
	307
	308	if (mac->link_state != MAC80211_LINKED)
	309	return;
	310
	311	if (ppsc->dot11_psmode == rt_psmode)
	312	return;
	313
	314	/* Update power save mode configured. */
	315	ppsc->dot11_psmode = rt_psmode;
	316
	317	/*
	318	*<FW control LPS>
	319	*1. Enter PS mode
	320	* Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
	321	* cmd to set Fw into PS mode.
	322	*2. Leave PS mode
	323	* Send H2C fw_pwrmode cmd to Fw to set Fw into Active
	324	* mode and set RPWM to turn RF on.
	325	*/
	326
cc7dc0c4	327	if ((ppsc->fwctrl_lps) && ppsc->report_linked) {
7ea47240	328	bool fw_current_inps;
0c817338 LF	329	if (ppsc->dot11_psmode == EACTIVE) {
0c817338 LF	330	RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
f30d7507 JP	331	"FW LPS leave ps_mode:%x\n",
f30d7507 JP	332	FW_PS_ACTIVE_MODE);
0c817338 LF	333
	334	rpwm_val = 0x0C; /* RF on */
	335	fw_pwrmode = FW_PS_ACTIVE_MODE;
	336	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
	337	(u8 *) (&rpwm_val));
	338	rtlpriv->cfg->ops->set_hw_reg(hw,
	339	HW_VAR_H2C_FW_PWRMODE,
	340	(u8 *) (&fw_pwrmode));
7ea47240	341	fw_current_inps = false;
0c817338 LF	342
	343	rtlpriv->cfg->ops->set_hw_reg(hw,
	344	HW_VAR_FW_PSMODE_STATUS,
7ea47240	345	(u8 *) (&fw_current_inps));
0c817338 LF	346
	347	} else {
	348	if (rtl_get_fwlps_doze(hw)) {
	349	RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
f30d7507 JP	350	"FW LPS enter ps_mode:%x\n",
f30d7507 JP	351	ppsc->fwctrl_psmode);
0c817338 LF	352
0c817338 LF	353	rpwm_val = 0x02; /* RF off */
7ea47240	354	fw_current_inps = true;
0c817338 LF	355	rtlpriv->cfg->ops->set_hw_reg(hw,
0c817338 LF	356	HW_VAR_FW_PSMODE_STATUS,
7ea47240	357	(u8 *) (&fw_current_inps));
0c817338 LF	358	rtlpriv->cfg->ops->set_hw_reg(hw,
	359	HW_VAR_H2C_FW_PWRMODE,
	360	(u8 *) (&ppsc->fwctrl_psmode));
	361
	362	rtlpriv->cfg->ops->set_hw_reg(hw,
	363	HW_VAR_SET_RPWM,
	364	(u8 *) (&rpwm_val));
	365	} else {
	366	/* Reset the power save related parameters. */
	367	ppsc->dot11_psmode = EACTIVE;
	368	}
	369	}
	370	}
	371	}
	372
	373	/Enter the leisure power save mode./
	374	void rtl_lps_enter(struct ieee80211_hw *hw)
	375	{
	376	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	377	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	378	struct rtl_priv *rtlpriv = rtl_priv(hw);
0c817338	379
cc7dc0c4	380	if (!ppsc->fwctrl_lps)
0c817338 LF	381	return;
	382
	383	if (rtlpriv->sec.being_setkey)
	384	return;
	385
7ea47240	386	if (rtlpriv->link_info.busytraffic)
0c817338 LF	387	return;
	388
	389	/sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! /
	390	if (mac->cnt_after_linked < 5)
	391	return;
	392
	393	if (mac->opmode == NL80211_IFTYPE_ADHOC)
	394	return;
	395
	396	if (mac->link_state != MAC80211_LINKED)
	397	return;
	398
6539306b	399	mutex_lock(&rtlpriv->locks.ps_mutex);
0c817338	400
cc7dc0c4 C	401	/* Idle for a while if we connect to AP a while ago. */
	402	if (mac->cnt_after_linked >= 2) {
	403	if (ppsc->dot11_psmode == EACTIVE) {
	404	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
f30d7507	405	"Enter 802.11 power save mode...\n");
0c817338	406
cc7dc0c4	407	rtl_lps_set_psmode(hw, EAUTOPS);
0c817338 LF	408	}
0c817338 LF	409	}
cc7dc0c4	410
6539306b	411	mutex_unlock(&rtlpriv->locks.ps_mutex);
0c817338 LF	412	}
	413
	414	/Leave the leisure power save mode./
	415	void rtl_lps_leave(struct ieee80211_hw *hw)
	416	{
	417	struct rtl_priv *rtlpriv = rtl_priv(hw);
	418	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	419	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
0c817338	420
6539306b	421	mutex_lock(&rtlpriv->locks.ps_mutex);
0c817338	422
cc7dc0c4	423	if (ppsc->fwctrl_lps) {
0c817338 LF	424	if (ppsc->dot11_psmode != EACTIVE) {
	425
	426	/FIX ME /
	427	rtlpriv->cfg->ops->enable_interrupt(hw);
	428
	429	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
cc7dc0c4	430	RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
0c817338 LF	431	rtlhal->interface == INTF_PCI) {
0c817338 LF	432	rtlpriv->intf_ops->disable_aspm(hw);
cc7dc0c4	433	RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
0c817338 LF	434	}
	435
	436	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
f30d7507	437	"Busy Traffic,Leave 802.11 power save..\n");
0c817338 LF	438
	439	rtl_lps_set_psmode(hw, EACTIVE);
	440	}
	441	}
6539306b	442	mutex_unlock(&rtlpriv->locks.ps_mutex);
0c817338	443	}
cc7dc0c4 C	444
	445	/* For sw LPS*/
	446	void rtl_swlps_beacon(struct ieee80211_hw hw, void data, unsigned int len)
	447	{
	448	struct rtl_priv *rtlpriv = rtl_priv(hw);
	449	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	450	struct ieee80211_hdr hdr = (void ) data;
	451	struct ieee80211_tim_ie *tim_ie;
	452	u8 *tim;
	453	u8 tim_len;
	454	bool u_buffed;
	455	bool m_buffed;
	456
	457	if (mac->opmode != NL80211_IFTYPE_STATION)
	458	return;
	459
	460	if (!rtlpriv->psc.swctrl_lps)
	461	return;
	462
	463	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
	464	return;
	465
	466	if (!rtlpriv->psc.sw_ps_enabled)
	467	return;
	468
	469	if (rtlpriv->psc.fwctrl_lps)
	470	return;
	471
	472	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
	473	return;
	474
	475	/* check if this really is a beacon */
	476	if (!ieee80211_is_beacon(hdr->frame_control))
	477	return;
	478
	479	/* min. beacon length + FCS_LEN */
	480	if (len <= 40 + FCS_LEN)
	481	return;
	482
	483	/* and only beacons from the associated BSSID, please */
	484	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
	485	return;
	486
	487	rtlpriv->psc.last_beacon = jiffies;
	488
	489	tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
	490	if (!tim)
	491	return;
	492
	493	if (tim[1] < sizeof(*tim_ie))
	494	return;
	495
	496	tim_len = tim[1];
	497	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
	498
	499	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
	500	rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
	501
	502	/* Check whenever the PHY can be turned off again. */
	503
	504	/* 1. What about buffered unicast traffic for our AID? */
	505	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
	506	rtlpriv->mac80211.assoc_id);
	507
508	/* 2. Maybe the AP wants to send multicast/broadcast data? */
509	m_buffed = tim_ie->bitmap_ctrl & 0x01;
510	rtlpriv->psc.multi_buffered = m_buffed;
511
512	/* unicast will process by mac80211 through
513	* set ~IEEE80211_CONF_PS, So we just check
514	* multicast frames here */
515	if (!m_buffed) {
516	/* back to low-power land. and delay is
517	* prevent null power save frame tx fail */
518	queue_delayed_work(rtlpriv->works.rtl_wq,
519	&rtlpriv->works.ps_work, MSECS(5));
520	} else {
f30d7507 JP	521	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
f30d7507 JP	522	"u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
cc7dc0c4 C	523	}
	524	}
	525
	526	void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
	527	{
	528	struct rtl_priv *rtlpriv = rtl_priv(hw);
	529	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
	530	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
cc7dc0c4 C	531
	532	if (!rtlpriv->psc.swctrl_lps)
	533	return;
	534	if (mac->link_state != MAC80211_LINKED)
	535	return;
	536
	537	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
	538	RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
	539	rtlpriv->intf_ops->disable_aspm(hw);
	540	RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
	541	}
	542
6539306b	543	mutex_lock(&rtlpriv->locks.ps_mutex);
4b9d8d67	544	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
6539306b	545	mutex_unlock(&rtlpriv->locks.ps_mutex);
cc7dc0c4 C	546	}
	547
	548	void rtl_swlps_rfon_wq_callback(void *data)
	549	{
	550	struct rtl_works *rtlworks =
	551	container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
	552	struct ieee80211_hw *hw = rtlworks->hw;
	553
	554	rtl_swlps_rf_awake(hw);
	555	}
	556
	557	void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
	558	{
	559	struct rtl_priv *rtlpriv = rtl_priv(hw);
	560	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
	561	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
cc7dc0c4 C	562	u8 sleep_intv;
	563
	564	if (!rtlpriv->psc.sw_ps_enabled)
	565	return;
	566
	567	if ((rtlpriv->sec.being_setkey) \|\|
	568	(mac->opmode == NL80211_IFTYPE_ADHOC))
	569	return;
	570
	571	/sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! /
	572	if ((mac->link_state != MAC80211_LINKED) \|\| (mac->cnt_after_linked < 5))
	573	return;
	574
	575	if (rtlpriv->link_info.busytraffic)
	576	return;
	577
6539306b	578	mutex_lock(&rtlpriv->locks.ps_mutex);
4b9d8d67	579	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
6539306b	580	mutex_unlock(&rtlpriv->locks.ps_mutex);
cc7dc0c4 C	581
	582	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
	583	!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
	584	rtlpriv->intf_ops->enable_aspm(hw);
	585	RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
	586	}
	587
	588	/* here is power save alg, when this beacon is DTIM
	589	* we will set sleep time to dtim_period * n;
	590	* when this beacon is not DTIM, we will set sleep
	591	* time to sleep_intv = rtlpriv->psc.dtim_counter or
	592	* MAX_SW_LPS_SLEEP_INTV(default set to 5) */
	593
	594	if (rtlpriv->psc.dtim_counter == 0) {
	595	if (hw->conf.ps_dtim_period == 1)
	596	sleep_intv = hw->conf.ps_dtim_period * 2;
	597	else
	598	sleep_intv = hw->conf.ps_dtim_period;
	599	} else {
	600	sleep_intv = rtlpriv->psc.dtim_counter;
	601	}
	602
	603	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
	604	sleep_intv = MAX_SW_LPS_SLEEP_INTV;
	605
	606	/* this print should always be dtim_conter = 0 &
	607	* sleep = dtim_period, that meaons, we should
	608	* awake before every dtim */
	609	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
f30d7507 JP	610	"dtim_counter:%x will sleep :%d beacon_intv\n",
f30d7507 JP	611	rtlpriv->psc.dtim_counter, sleep_intv);
cc7dc0c4 C	612
	613	/* we tested that 40ms is enough for sw & hw sw delay */
	614	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
	615	MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
	616	}
	617
	618
	619	void rtl_swlps_wq_callback(void *data)
	620	{
	621	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
	622	struct rtl_works,
	623	ps_work);
	624	struct ieee80211_hw *hw = rtlworks->hw;
	625	struct rtl_priv *rtlpriv = rtl_priv(hw);
	626	bool ps = false;
	627
	628	ps = (hw->conf.flags & IEEE80211_CONF_PS);
	629
	630	/* we can sleep after ps null send ok */
	631	if (rtlpriv->psc.state_inap) {
	632	rtl_swlps_rf_sleep(hw);
	633
	634	if (rtlpriv->psc.state && !ps) {
	635	rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
	636	rtlpriv->psc.last_action);
	637	}
	638
	639	if (ps)
	640	rtlpriv->psc.last_slept = jiffies;
	641
	642	rtlpriv->psc.last_action = jiffies;
	643	rtlpriv->psc.state = ps;
	644	}
	645	}