/* SPDX-License-Identifier: GPL-2.0 */
/******************************************************************************
 *
 * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
 *
 *
 ******************************************************************************/
#define _RTW_PWRCTRL_C_

#include <drv_types.h>
#include <hal_data.h>
#include <hal_com_h2c.h>

int rtw_fw_ps_state(PADAPTER padapter)
{
	struct dvobj_priv *psdpriv = padapter->dvobj;
	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
	int ret = _FAIL, dont_care = 0;
	u16 fw_ps_state = 0;
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	struct registry_priv  *registry_par = &padapter->registrypriv;

	if (registry_par->check_fw_ps != 1)
		return _SUCCESS;

	_enter_pwrlock(&pwrpriv->check_32k_lock);

	if (RTW_CANNOT_RUN(padapter)) {
		RTW_INFO("%s: bSurpriseRemoved=%s , hw_init_completed=%d, bDriverStopped=%s\n", __func__
			 , rtw_is_surprise_removed(padapter) ? "True" : "False"
			 , rtw_get_hw_init_completed(padapter)
			 , rtw_is_drv_stopped(padapter) ? "True" : "False");
		goto exit_fw_ps_state;
	}
	rtw_hal_set_hwreg(padapter, HW_VAR_SET_REQ_FW_PS, (u8 *)&dont_care);
	{
		/* 4. if 0x88[7]=1, driver set cmd to leave LPS/IPS. */
		/* Else, hw will keep in active mode. */
		/* debug info: */
		/* 0x88[7] = 32kpermission, */
		/* 0x88[6:0] = current_ps_state */
		/* 0x89[7:0] = last_rpwm */

		rtw_hal_get_hwreg(padapter, HW_VAR_FW_PS_STATE, (u8 *)&fw_ps_state);

		if ((fw_ps_state & 0x80) == 0)
			ret = _SUCCESS;
		else {
			pdbgpriv->dbg_poll_fail_cnt++;
			RTW_INFO("%s: fw_ps_state=%04x\n", __FUNCTION__, fw_ps_state);
		}
	}

exit_fw_ps_state:
	_exit_pwrlock(&pwrpriv->check_32k_lock);
	return ret;
}

void _ips_enter(_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++;
	RTW_INFO("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);

	if (rf_off == pwrpriv->change_rfpwrstate) {
		pwrpriv->bpower_saving = _TRUE;
		RTW_PRINT("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(_adapter *padapter)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);

	rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);

	_enter_pwrlock(&pwrpriv->lock);
	_ips_enter(padapter);
	_exit_pwrlock(&pwrpriv->lock);
}

int _ips_leave(_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++;
		RTW_INFO("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);

		result = rtw_ips_pwr_up(padapter);
		if (result == _SUCCESS)
			pwrpriv->rf_pwrstate = rf_on;
		RTW_PRINT("nolinked power save leave\n");

		RTW_INFO("==> 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(_adapter *padapter)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	int ret;

	if (!is_primary_adapter(padapter))
		return _SUCCESS;

	_enter_pwrlock(&pwrpriv->lock);
	ret = _ips_leave(padapter);
	_exit_pwrlock(&pwrpriv->lock);

	if (_SUCCESS == ret)
		odm_dm_reset(&GET_HAL_DATA(padapter)->odmpriv);

	if (_SUCCESS == ret)
		rtw_btcoex_IpsNotify(padapter, IPS_NONE);

	return ret;
}

bool rtw_pwr_unassociated_idle(_adapter *adapter)
{
	u8 i;
	_adapter *iface;
	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
	struct mlme_priv *pmlmepriv;
	struct wifidirect_info	*pwdinfo;

	bool ret = _FALSE;

	if (adapter_to_pwrctl(adapter)->bpower_saving == _TRUE) {
		/* RTW_INFO("%s: already in LPS or IPS mode\n", __func__); */
		goto exit;
	}

	if (adapter_to_pwrctl(adapter)->ips_deny_time >= rtw_get_current_time()) {
		/* RTW_INFO("%s ips_deny_time\n", __func__); */
		goto exit;
	}

	for (i = 0; i < dvobj->iface_nums; i++) {
		iface = dvobj->padapters[i];
		if ((iface) && rtw_is_adapter_up(iface)) {
			pmlmepriv = &(iface->mlmepriv);
			pwdinfo = &(iface->wdinfo);
			if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_SITE_MONITOR)
			    || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS)
			    || check_fwstate(pmlmepriv, WIFI_AP_STATE)
			    || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE)
			    || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)
			    || rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)
			   )
				goto exit;

		}
	}

	if (adapter->registrypriv.mp_mode == 1)
		goto exit;


	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
	    pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
		RTW_PRINT("There are some pkts to transmit\n");
		RTW_PRINT("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(_adapter *padapter)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
	struct dvobj_priv *psdpriv = padapter->dvobj;
	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
	u32 ps_deny = 0;

	_enter_pwrlock(&adapter_to_pwrctl(padapter)->lock);
	ps_deny = rtw_ps_deny_get(padapter);
	_exit_pwrlock(&adapter_to_pwrctl(padapter)->lock);
	if (ps_deny != 0) {
		RTW_INFO(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++;
		RTW_INFO("%s, pwrpriv->bInSuspend == _TRUE ignore this process\n", __FUNCTION__);
		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)) {
		RTW_INFO("==>%s .fw_state(%x)\n", __FUNCTION__, get_fwstate(pmlmepriv));
		pwrpriv->change_rfpwrstate = rf_off;
		{

			ips_enter(padapter);
		}
	}
exit:
#ifndef CONFIG_IPS_CHECK_IN_WD
	rtw_set_pwr_state_check_timer(pwrpriv);
#endif
	pwrpriv->ps_processing = _FALSE;
	return;
}

void pwr_state_check_handler(void *ctx)
{
	_adapter *padapter = (_adapter *)ctx;
	rtw_ps_cmd(padapter);
}

/*
 * 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(PADAPTER padapter, u8 pslv)
{
	u8	rpwm;
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);

	pslv = PS_STATE(pslv);

	{
		if ((pwrpriv->rpwm == pslv)
			|| (pwrpriv->lps_level == LPS_NORMAL)
		   ) {
			return;
		}
	}

	if (rtw_is_surprise_removed(padapter) ||
	    (!rtw_is_hw_init_completed(padapter))) {

		pwrpriv->cpwm = PS_STATE_S4;

		return;
	}

	if (rtw_is_drv_stopped(padapter))
		if (pslv < PS_STATE_S2)
			return;

	rpwm = pslv | pwrpriv->tog;

	pwrpriv->rpwm = pslv;


	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));

	pwrpriv->tog += 0x80;

	{
		pwrpriv->cpwm = pslv;
	}

}

u8 PS_RDY_CHECK(_adapter *padapter)
{
	u32 curr_time, delta_time;
	struct pwrctrl_priv	*pwrpriv = adapter_to_pwrctl(padapter);
	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

	if (_TRUE == pwrpriv->bInSuspend)
		return _FALSE;

	curr_time = rtw_get_current_time();

	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)) {
		RTW_INFO("Group handshake still in progress !!!\n");
		return _FALSE;
	}

	return _TRUE;
}

#if defined(CONFIG_FWLPS_IN_IPS)
void rtw_set_fw_in_ips_mode(PADAPTER padapter, u8 enable)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	int cnt = 0;
	u32 start_time;
	u8 val8 = 0;
	u8 cpwm_orig = 0, cpwm_now = 0;
	u8 parm[H2C_INACTIVE_PS_LEN] = {0};

	if (padapter->netif_up == _FALSE) {
		RTW_INFO("%s: ERROR, netif is down\n", __func__);
		return;
	}

	/* u8 cmd_param; */ /* BIT0:enable, BIT1:NoConnect32k */
	if (enable) {
		rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
		/* Enter IPS */
		RTW_INFO("%s: issue H2C to FW when entering IPS\n", __func__);

		parm[0] = 0x1;/* suggest by Isaac.Hsu*/

		rtw_hal_fill_h2c_cmd(padapter, /* H2C_FWLPS_IN_IPS_, */
				     H2C_INACTIVE_PS_,
				     H2C_INACTIVE_PS_LEN, parm);
		/* poll 0x1cc to make sure H2C command already finished by FW; MAC_0x1cc=0 means H2C done by FW. */
		do {
			val8 = rtw_read8(padapter, REG_HMETFR);
			cnt++;
			RTW_INFO("%s  polling REG_HMETFR=0x%x, cnt=%d\n",
				 __func__, val8, cnt);
			rtw_mdelay_os(10);
		} while (cnt < 100 && (val8 != 0));

	} else {
		/* Leave IPS */
		RTW_INFO("%s: Leaving IPS in FWLPS state\n", __func__);

		parm[0] = 0x0;
		parm[1] = 0x0;
		parm[2] = 0x0;
		rtw_hal_fill_h2c_cmd(padapter, H2C_INACTIVE_PS_,
				     H2C_INACTIVE_PS_LEN, parm);
		rtw_btcoex_IpsNotify(padapter, IPS_NONE);
	}
}
#endif /* CONFIG_PNO_SUPPORT */

void rtw_set_ps_mode(PADAPTER padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);

	if (ps_mode > PM_Card_Disable) {
		return;
	}

	if (pwrpriv->pwr_mode == ps_mode) {
		if (PS_MODE_ACTIVE == ps_mode)
			return;

	}

	if (ps_mode == PS_MODE_ACTIVE) {
		if (1
		    && (((rtw_btcoex_IsBtControlLps(padapter) == _FALSE)
			 && (pwdinfo->opp_ps == 0)
			)
			|| ((rtw_btcoex_IsBtControlLps(padapter) == _TRUE)
			    && (rtw_btcoex_IsLpsOn(padapter) == _FALSE))
		       )
		   ) {
			RTW_INFO(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n",
				 FUNC_ADPT_ARG(padapter), msg);

			if (pwrpriv->lps_leave_cnts < UINT_MAX)
				pwrpriv->lps_leave_cnts++;
			else
				pwrpriv->lps_leave_cnts = 0;

			pwrpriv->pwr_mode = ps_mode;
			rtw_set_rpwm(padapter, PS_STATE_S4);

			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));



			pwrpriv->bFwCurrentInPSMode = _FALSE;

			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;

			RTW_INFO(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n",
				 FUNC_ADPT_ARG(padapter), msg);

			if (pwrpriv->lps_enter_cnts < UINT_MAX)
				pwrpriv->lps_enter_cnts++;
			else
				pwrpriv->lps_enter_cnts = 0;

			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));

			/* Set CTWindow after LPS */
			if (pwdinfo->opp_ps == 1)
				p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0);

			pslv = PS_STATE_S2;

			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);
		}
	}
}

/*
 * Return:
 *	0:	Leave OK
 *	-1:	Timeout
 *	-2:	Other error
 */
s32 LPS_RF_ON_check(PADAPTER padapter, u32 delay_ms)
{
	u32 start_time;
	u8 bAwake = _FALSE;
	s32 err = 0;

	start_time = rtw_get_current_time();
	while (1) {
		rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
		if (_TRUE == bAwake)
			break;

		if (rtw_is_surprise_removed(padapter)) {
			err = -2;
			RTW_INFO("%s: device surprise removed!!\n", __FUNCTION__);
			break;
		}

		if (rtw_get_passing_time_ms(start_time) > delay_ms) {
			err = -1;
			RTW_INFO("%s: Wait for FW LPS leave more than %u ms!!!\n", __FUNCTION__, delay_ms);
			break;
		}
		rtw_usleep_os(100);
	}

	return err;
}

/*
 *	Description:
 *		Enter the leisure power save mode.
 *   */
void LPS_Enter(PADAPTER 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;
	int i;
	char buf[32] = {0};

	/*	RTW_INFO("+LeisurePSEnter\n"); */
	if (_FALSE == padapter->bFWReady)
		return;

	if (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)
		return;

	/* Skip lps enter request if number of assocated adapters is not 1 */
	for (i = 0; i < dvobj->iface_nums; i++) {
		if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
			n_assoc_iface++;
	}
	if (n_assoc_iface != 1)
		return;

	/* Skip lps enter request for adapter not port0 */
	if (get_hw_port(padapter) != HW_PORT0)
		return;

	for (i = 0; i < dvobj->iface_nums; i++) {
		if (PS_RDY_CHECK(dvobj->padapters[i]) == _FALSE)
			return;
	}

	if (padapter->wdinfo.p2p_ps_mode == P2P_PS_NOA) {
		return;/* supporting p2p client ps NOA via H2C_8723B_P2P_PS_OFFLOAD */
	}

	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++;
	}

	/*	RTW_INFO("-LeisurePSEnter\n"); */

}

/*
 *	Description:
 *		Leave the leisure power save mode.
 *   */
void LPS_Leave(PADAPTER 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};

	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;
}

void LeaveAllPowerSaveModeDirect(PADAPTER Adapter)
{
	PADAPTER pri_padapter = GET_PRIMARY_ADAPTER(Adapter);
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter);

	RTW_INFO("%s.....\n", __FUNCTION__);

	if (rtw_is_surprise_removed(Adapter)) {
		RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=_TRUE Skip!\n", FUNC_ADPT_ARG(Adapter));
		return;
	}

	if (rtw_mi_check_status(Adapter, MI_LINKED)) { /*connect*/

		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
			RTW_INFO("%s: Driver Already Leave LPS\n", __FUNCTION__);
			return;
		}


		p2p_ps_wk_cmd(pri_padapter, P2P_PS_DISABLE, 0);

		rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
	} else {
		if (pwrpriv->rf_pwrstate == rf_off) {
			{
#if defined(CONFIG_FWLPS_IN_IPS) || defined(CONFIG_SWLPS_IN_IPS)
				if (_FALSE == ips_leave(pri_padapter))
					RTW_INFO("======> ips_leave fail.............\n");
#endif /* CONFIG_SWLPS_IN_IPS || (CONFIG_PLATFORM_SPRD && CONFIG_RTL8188E) */
			}
		}
	}
}

/*
 * Description: Leave all power save mode: LPS, FwLPS, IPS if needed.
 * Move code to function by tynli. 2010.03.26.
 *   */
void LeaveAllPowerSaveMode(IN PADAPTER Adapter)
{
	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
	u8	enqueue = 0;
	int n_assoc_iface = 0;
	int i;

	/* RTW_INFO("%s.....\n",__FUNCTION__); */

	if (_FALSE == Adapter->bup) {
		RTW_INFO(FUNC_ADPT_FMT ": bup=%d Skip!\n",
			 FUNC_ADPT_ARG(Adapter), Adapter->bup);
		return;
	}

	if (rtw_is_surprise_removed(Adapter)) {
		RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=_TRUE Skip!\n", FUNC_ADPT_ARG(Adapter));
		return;
	}

	for (i = 0; i < dvobj->iface_nums; i++) {
		if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
			n_assoc_iface++;
	}

	if (n_assoc_iface) {
		/* connect */

		p2p_ps_wk_cmd(Adapter, P2P_PS_DISABLE, enqueue);

		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);

	} else {
		if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) {
			{
#if defined(CONFIG_FWLPS_IN_IPS) || defined(CONFIG_SWLPS_IN_IPS)
				if (_FALSE == ips_leave(Adapter))
					RTW_INFO("======> ips_leave fail.............\n");
#endif /* CONFIG_SWLPS_IN_IPS || (CONFIG_PLATFORM_SPRD && CONFIG_RTL8188E) */
			}
		}
	}
}



void rtw_init_pwrctrl_priv(PADAPTER padapter)
{
	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);

	_init_pwrlock(&pwrctrlpriv->lock);
	_init_pwrlock(&pwrctrlpriv->check_32k_lock);
	pwrctrlpriv->rf_pwrstate = rf_on;
	pwrctrlpriv->ips_enter_cnts = 0;
	pwrctrlpriv->ips_leave_cnts = 0;
	pwrctrlpriv->lps_enter_cnts = 0;
	pwrctrlpriv->lps_leave_cnts = 0;
	pwrctrlpriv->bips_processing = _FALSE;

	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
	pwrctrlpriv->lps_level = padapter->registrypriv.lps_level;

	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->FWCtrlPSMode =padapter->registrypriv.power_mgnt; */ /* PS_MODE_MIN; */
	if (padapter->registrypriv.mp_mode == 1)
		pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE ;
	else
		pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt; /* PS_MODE_MIN; */
	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? _TRUE : _FALSE;

	pwrctrlpriv->bFwCurrentInPSMode = _FALSE;

	pwrctrlpriv->rpwm = 0;
	pwrctrlpriv->cpwm = PS_STATE_S4;

	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
	pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
	pwrctrlpriv->bcn_ant_mode = 0;
	pwrctrlpriv->dtim = 0;

	pwrctrlpriv->tog = 0x80;


	rtw_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter, pwr_state_check_handler, padapter);

	pwrctrlpriv->wowlan_mode = _FALSE;
	pwrctrlpriv->wowlan_ap_mode = _FALSE;
	pwrctrlpriv->wowlan_p2p_mode = _FALSE;
	pwrctrlpriv->wowlan_last_wake_reason = 0;
}

void rtw_free_pwrctrl_priv(PADAPTER adapter)
{
	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(adapter);

	_free_pwrlock(&pwrctrlpriv->lock);
	_free_pwrlock(&pwrctrlpriv->check_32k_lock);
}

inline void rtw_set_ips_deny(_adapter *padapter, u32 ms)
{
	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
	pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ms);
}

/*
* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
* @adapter: pointer to _adapter structure
* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup
* Return _SUCCESS or _FAIL
*/

int _rtw_pwr_wakeup(_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;
	u32 start = rtw_get_current_time();

	/* for LPS */
	LeaveAllPowerSaveMode(padapter);

	/* IPS still bound with primary adapter */
	padapter = GET_PRIMARY_ADAPTER(padapter);
	pmlmepriv = &padapter->mlmepriv;

	if (pwrpriv->ips_deny_time < rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms))
		pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms);

	if (pwrpriv->ps_processing) {
		RTW_INFO("%s wait ps_processing...\n", __func__);
		while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
			rtw_msleep_os(10);
		if (pwrpriv->ps_processing)
			RTW_INFO("%s wait ps_processing timeout\n", __func__);
		else
			RTW_INFO("%s wait ps_processing done\n", __func__);
	}

	if (rtw_hal_sreset_inprogress(padapter)) {
		RTW_INFO("%s wait sreset_inprogress...\n", __func__);
		while (rtw_hal_sreset_inprogress(padapter) && rtw_get_passing_time_ms(start) <= 4000)
			rtw_msleep_os(10);
		if (rtw_hal_sreset_inprogress(padapter))
			RTW_INFO("%s wait sreset_inprogress timeout\n", __func__);
		else
			RTW_INFO("%s wait sreset_inprogress done\n", __func__);
	}

	if (pwrpriv->bInternalAutoSuspend == _FALSE && pwrpriv->bInSuspend) {
		RTW_INFO("%s wait bInSuspend...\n", __func__);
		while (pwrpriv->bInSuspend
		       && ((rtw_get_passing_time_ms(start) <= 3000 && !rtw_is_do_late_resume(pwrpriv))
			|| (rtw_get_passing_time_ms(start) <= 500 && rtw_is_do_late_resume(pwrpriv)))
		      )
			rtw_msleep_os(10);
		if (pwrpriv->bInSuspend)
			RTW_INFO("%s wait bInSuspend timeout\n", __func__);
		else
			RTW_INFO("%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) {
		{
			RTW_INFO("%s call ips_leave....\n", __FUNCTION__);
			if (_FAIL ==  ips_leave(padapter)) {
				RTW_INFO("======> ips_leave fail.............\n");
				ret = _FAIL;
				goto exit;
			}
		}
	}

	/* TODO: the following checking need to be merged... */
	if (rtw_is_drv_stopped(padapter)
	    || !padapter->bup
	    || !rtw_is_hw_init_completed(padapter)
	   ) {
		RTW_INFO("%s: bDriverStopped=%s, bup=%d, hw_init_completed=%u\n"
			 , caller
			 , rtw_is_drv_stopped(padapter) ? "True" : "False"
			 , padapter->bup
			 , rtw_get_hw_init_completed(padapter));
		ret = _FALSE;
		goto exit;
	}

exit:
	if (pwrpriv->ips_deny_time < rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms))
		pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms);
	return ret;

}

int rtw_pm_set_lps(_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_lps_level(_adapter *padapter, u8 level)
{
	int	ret = 0;
	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);

	if (level < LPS_LEVEL_MAX)
		pwrctrlpriv->lps_level = level;
	else
		ret = -EINVAL;

	return ret;
}

int rtw_pm_set_ips(_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);
		RTW_INFO("%s %s\n", __FUNCTION__, mode == IPS_NORMAL ? "IPS_NORMAL" : "IPS_LEVEL_2");
		return 0;
	} else if (mode == IPS_NONE) {
		rtw_ips_mode_req(pwrctrlpriv, mode);
		RTW_INFO("%s %s\n", __FUNCTION__, "IPS_NONE");
		if (!rtw_is_surprise_removed(padapter) && (_FAIL == rtw_pwr_wakeup(padapter)))
			return -EFAULT;
	} else
		return -EINVAL;
	return 0;
}

/*
 * ATTENTION:
 *	This function will request pwrctrl LOCK!
 */
void rtw_ps_deny(PADAPTER padapter, PS_DENY_REASON reason)
{
	struct pwrctrl_priv *pwrpriv;

	/* 	RTW_INFO("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n",
	 *		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */

	pwrpriv = adapter_to_pwrctl(padapter);

	_enter_pwrlock(&pwrpriv->lock);
	if (pwrpriv->ps_deny & BIT(reason)) {
		RTW_INFO(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n",
			 FUNC_ADPT_ARG(padapter), reason);
	}
	pwrpriv->ps_deny |= BIT(reason);
	_exit_pwrlock(&pwrpriv->lock);

	/* 	RTW_INFO("-" 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(PADAPTER padapter, PS_DENY_REASON reason)
{
	struct pwrctrl_priv *pwrpriv;

	/* 	RTW_INFO("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n",
	 *		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */

	pwrpriv = adapter_to_pwrctl(padapter);

	_enter_pwrlock(&pwrpriv->lock);
	if ((pwrpriv->ps_deny & BIT(reason)) == 0) {
		RTW_INFO(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n",
			 FUNC_ADPT_ARG(padapter), reason);
	}
	pwrpriv->ps_deny &= ~BIT(reason);
	_exit_pwrlock(&pwrpriv->lock);

	/* 	RTW_INFO("-" 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(PADAPTER padapter)
{
	u32 deny;

	deny = adapter_to_pwrctl(padapter)->ps_deny;

	return deny;
}
