staging: rtl8723au: rtw_pwrctrl.c: Reduce spaghettification (d8ab7e84) · Commits · jan.koester / Linux

drivers/staging/rtl8723au/core/rtw_pwrctrl.c

+111 −104

Original line number	Diff line number	Diff line
		@@ -39,8 +39,7 @@ void ips_enter23a(struct rtw_adapter * padapter)
		#ifdef CONFIG_8723AU_BT_COEXIST
		BTDM_TurnOffBtCoexistBeforeEnterIPS(padapter);
		#endif
		if (rf_off == pwrpriv->change_rfpwrstate)
		{
		if (pwrpriv->change_rfpwrstate == rf_off) {
		pwrpriv->bpower_saving = true;
		DBG_8723A_LEVEL(_drv_always_, "nolinked power save enter\n");

		@@ -65,25 +64,32 @@ int ips_leave23a(struct rtw_adapter * padapter)

		down(&pwrpriv->lock);

		if ((pwrpriv->rf_pwrstate == rf_off) &&!pwrpriv->bips_processing)
		{
		if (pwrpriv->rf_pwrstate == rf_off && !pwrpriv->bips_processing) {
		pwrpriv->bips_processing = true;
		pwrpriv->change_rfpwrstate = rf_on;
		pwrpriv->ips_leave23a_cnts++;
		DBG_8723A("==>ips_leave23a cnts:%d\n", pwrpriv->ips_leave23a_cnts);
		DBG_8723A("==>ips_leave23a cnts:%d\n",
		pwrpriv->ips_leave23a_cnts);

		if ((result = rtw_ips_pwr_up23a(padapter)) == _SUCCESS) {
		result = rtw_ips_pwr_up23a(padapter);
		if (result == _SUCCESS)
		pwrpriv->rf_pwrstate = rf_on;
		}

		DBG_8723A_LEVEL(_drv_always_, "nolinked power save leave\n");

		if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) \|\|(_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm))
		{
		DBG_8723A("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing);
		set_channel_bwmode23a(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
		if (psecuritypriv->dot11PrivacyAlgrthm == _WEP40_ \|\|
		psecuritypriv->dot11PrivacyAlgrthm == _WEP104_) {
		DBG_8723A("==>%s, channel(%d), processing(%x)\n",
		__func__, padapter->mlmeextpriv.cur_channel,
		pwrpriv->bips_processing);
		set_channel_bwmode23a(padapter,
		padapter->mlmeextpriv.cur_channel,
		HAL_PRIME_CHNL_OFFSET_DONT_CARE,
		HT_CHANNEL_WIDTH_20);
		for (keyid = 0; keyid < 4; keyid++) {
		if (pmlmepriv->key_mask & BIT(keyid)) {
		if (keyid == psecuritypriv->dot11PrivacyKeyIndex)
		if (keyid ==
		psecuritypriv->dot11PrivacyKeyIndex)
		result = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
		else
		result = rtw_set_key23a(padapter, psecuritypriv, keyid, 0);
		@@ -91,7 +97,8 @@ int ips_leave23a(struct rtw_adapter * padapter)
		}
		}

		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n",
		rtw_read32(padapter, 0x4c));
		pwrpriv->bips_processing = false;

		pwrpriv->bkeepfwalive = false;
		@@ -139,9 +146,12 @@ static bool rtw_pwr_unassociated_idle(struct rtw_adapter *adapter)

		if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF \|\|
		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
		DBG_8723A_LEVEL(_drv_always_, "There are some pkts to transmit\n");
		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
		pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
		DBG_8723A_LEVEL(_drv_always_,
		"There are some pkts to transmit\n");
		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, "
		"free_xmit_extbuf_cnt: %d\n",
		pxmit_priv->free_xmitbuf_cnt,
		pxmit_priv->free_xmit_extbuf_cnt);
		goto exit;
		}

		@@ -164,7 +174,8 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)

		if (padapter->pwrctrlpriv.bHWPwrPindetect) {
		rfpwrstate = RfOnOffDetect23a(padapter);
		DBG_8723A("@@@@- #2 %s ==> rfstate:%s\n", __func__, (rfpwrstate == rf_on)?"rf_on":"rf_off");
		DBG_8723A("@@@@- #2 %s ==> rfstate:%s\n", __func__,
		(rfpwrstate == rf_on)?"rf_on":"rf_off");

		if (rfpwrstate!= pwrpriv->rf_pwrstate) {
		if (rfpwrstate == rf_off) {
		@@ -176,7 +187,9 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
		pwrpriv->change_rfpwrstate = rf_on;
		rtw_hw_resume23a(padapter);
		}
		DBG_8723A("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off)?"rf_off":"rf_on");
		DBG_8723A("current rf_pwrstate(%s)\n",
		(pwrpriv->rf_pwrstate == rf_off) ?
		"rf_off":"rf_on");
		}
		pwrpriv->pwr_state_check_cnts ++;
		}
		@@ -187,9 +200,10 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
		if (rtw_pwr_unassociated_idle(padapter) == false)
		goto exit;

		if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0))
		{
		DBG_8723A("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv));
		if (pwrpriv->rf_pwrstate == rf_on &&
		(pwrpriv->pwr_state_check_cnts % 4) == 0) {
		DBG_8723A("==>%s .fw_state(%x)\n", __func__,
		get_fwstate(pmlmepriv));
		pwrpriv->change_rfpwrstate = rf_off;
		ips_enter23a(padapter);
		}
		@@ -217,27 +231,26 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
		u8 rpwm;
		struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;



		pslv = PS_STATE(pslv);

		if (true == pwrpriv->btcoex_rfon)
		{
		if (pwrpriv->btcoex_rfon) {
		if (pslv < PS_STATE_S4)
		pslv = PS_STATE_S3;
		}

		if (pwrpriv->rpwm == pslv) {
		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
		("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
		("%s: Already set rpwm[0x%02X], new = 0x%02X!\n",
		__func__, pwrpriv->rpwm, pslv));
		return;
		}

		if ((padapter->bSurpriseRemoved == true) \|\|
		(padapter->hw_init_completed == false)) {
		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));
		__func__, padapter->bSurpriseRemoved,
		padapter->hw_init_completed));

		pwrpriv->cpwm = PS_STATE_S4;

		@@ -246,18 +259,22 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)

		if (padapter->bDriverStopped == true) {
		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
		("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
		("%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));
		("%s: Reject to enter PS_STATE(0x%02X) lower "
		"than S2 when DriverStopped!!\n",
		__func__, pslv));
		return;
		}
		}

		rpwm = pslv \| pwrpriv->tog;
		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
		("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
		("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n",
		rpwm, pwrpriv->cpwm));

		pwrpriv->rpwm = pslv;

		@@ -265,8 +282,6 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)

		pwrpriv->tog += 0x80;
		pwrpriv->cpwm = pslv;


		}

		static u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
		@@ -278,20 +293,18 @@ static u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
		delta_time = jiffies - pwrpriv->DelayLPSLastTimeStamp;

		if (delta_time < LPS_DELAY_TIME)
		{
		return false;
		}

		if ((check_fwstate(pmlmepriv, _FW_LINKED) == false) \|\|
		(check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) \|\|
		(check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) \|\|
		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) \|\|
		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
		if (check_fwstate(pmlmepriv, _FW_LINKED) == false \|\|
		check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true \|\|
		check_fwstate(pmlmepriv, WIFI_AP_STATE) == true \|\|
		check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true \|\|
		check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)
		return false;
		if (true == pwrpriv->bInSuspend)
		if (pwrpriv->bInSuspend)
		return false;
		if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false))
		{
		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X &&
		padapter->securitypriv.binstallGrpkey == false) {
		DBG_8723A("Group handshake still in progress !!!\n");
		return false;
		}
		@@ -301,7 +314,8 @@ static u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
		return true;
		}

		void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
		void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode,
		u8 smart_ps, u8 bcn_ant_mode)
		{
		struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;

		@@ -310,20 +324,19 @@ void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode, u8 smart_ps, u
		__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));
		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
		("ps_mode:%d error\n", ps_mode));
		return;
		}

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

		if ((pwrpriv->smart_ps == smart_ps) &&
		(pwrpriv->bcn_ant_mode == bcn_ant_mode))
		{
		if (pwrpriv->smart_ps == smart_ps &&
		pwrpriv->bcn_ant_mode == bcn_ant_mode)
		return;
		}
		}

		if (ps_mode == PS_MODE_ACTIVE) {
		DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n");
		@@ -380,7 +393,8 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)

		if (time_after(jiffies, end_time)) {
		err = -1;
		DBG_8723A("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
		DBG_8723A("%s: Wait for FW LPS leave more than %u "
		"ms!\n", __func__, delay_ms);
		break;
		}
		udelay(100);
		@@ -403,15 +417,17 @@ void LPS_Enter23a(struct rtw_adapter *padapter)
		if (pwrpriv->LpsIdleCount >= 2) { /* 4 Sec */
		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
		pwrpriv->bpower_saving = true;
		DBG_8723A("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps);
		DBG_8723A("%s smart_ps:%d\n", __func__,
		pwrpriv->smart_ps);
		/* For Tenda W311R IOT issue */
		rtw_set_ps_mode23a(padapter, pwrpriv->power_mgnt, pwrpriv->smart_ps, 0);
		rtw_set_ps_mode23a(padapter,
		pwrpriv->power_mgnt,
		pwrpriv->smart_ps, 0);
		}
		} else {
		} else
		pwrpriv->LpsIdleCount++;
		}
		}
		}

		/* Description: */
		/* Leave the leisure power save mode. */
		@@ -426,7 +442,8 @@ void LPS_Leave23a(struct rtw_adapter *padapter)
		rtw_set_ps_mode23a(padapter, PS_MODE_ACTIVE, 0, 0);

		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
		LPS_RF_ON_check23a(padapter, LPS_LEAVE_TIMEOUT_MS);
		LPS_RF_ON_check23a(padapter,
		LPS_LEAVE_TIMEOUT_MS);
		}
		}

		@@ -440,17 +457,11 @@ void LeaveAllPowerSaveMode23a(struct rtw_adapter *Adapter)
		struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
		u8 enqueue = 0;



		/* DBG_8723A("%s.....\n", __func__); */
		if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
		{ /* connect */
		rtw_lps_ctrl_wk_cmd23a(Adapter, LPS_CTRL_LEAVE, enqueue);
		}


		}

		void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
		{
		struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
		@@ -471,8 +482,11 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
		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;

		/* PS_MODE_MIN; */
		pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;
		pwrctrlpriv->bLeisurePs =
		(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;

		pwrctrlpriv->bFwCurrentInPSMode = false;

		@@ -489,8 +503,6 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)

		setup_timer(&pwrctrlpriv->pwr_state_check_timer,
		pwr_state_check_handler, (unsigned long)padapter);


		}

		void rtw_free_pwrctrl_priv(struct rtw_adapter *adapter)
		@@ -540,7 +552,8 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
		jiffies_to_msecs(jiffies - start) <= 4000)
		msleep(10);
		if (rtw_hal_sreset_inprogress(padapter))
		DBG_8723A("%s wait sreset_inprogress timeout\n", __func__);
		DBG_8723A("%s wait sreset_inprogress timeout\n",
		__func__);
		else
		DBG_8723A("%s wait sreset_inprogress done\n", __func__);
		}
		@@ -558,27 +571,28 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
		}

		/* System suspend is not allowed to wakeup */
		if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) {
		if (pwrpriv->bInternalAutoSuspend == false &&
		pwrpriv->bInSuspend == true) {
		ret = _FAIL;
		goto exit;
		}

		/* block??? */
		if ((pwrpriv->bInternalAutoSuspend == true) && (padapter->net_closed == true)) {
		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)
		{
		if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
		ret = _SUCCESS;
		goto exit;
		}

		if (rf_off == pwrpriv->rf_pwrstate) {
		DBG_8723A("%s call ips_leave23a....\n", __func__);
		if (_FAIL == ips_leave23a(padapter)) {
		if (ips_leave23a(padapter)== _FAIL) {
		DBG_8723A("======> ips_leave23a fail.............\n");
		ret = _FAIL;
		goto exit;
		@@ -607,26 +621,19 @@ int rtw_pm_set_lps23a(struct rtw_adapter *padapter, u8 mode)
		int ret = 0;
		struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;

		if (mode < PS_MODE_NUM)
		{
		if (pwrctrlpriv->power_mgnt != mode)
		{
		if (mode < PS_MODE_NUM) {
		if (pwrctrlpriv->power_mgnt != mode) {
		if (PS_MODE_ACTIVE == mode)
		{
		LeaveAllPowerSaveMode23a(padapter);
		}
		else
		{
		pwrctrlpriv->LpsIdleCount = 2;
		}
		pwrctrlpriv->power_mgnt = mode;
		pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
		pwrctrlpriv->bLeisurePs =
		(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ?
		true:false;
		}
		}
		else
		{
		} else
		ret = -EINVAL;
		}

		return ret;
		}
		@@ -637,17 +644,17 @@ int rtw_pm_set_ips23a(struct rtw_adapter *padapter, u8 mode)

		if (mode == IPS_NORMAL \|\| mode == IPS_LEVEL_2) {
		rtw_ips_mode_req(pwrctrlpriv, mode);
		DBG_8723A("%s %s\n", __func__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
		DBG_8723A("%s %s\n", __func__,
		mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
		return 0;
		}
		else if (mode == IPS_NONE) {
		} else if (mode == IPS_NONE) {
		rtw_ips_mode_req(pwrctrlpriv, mode);
		DBG_8723A("%s %s\n", __func__, "IPS_NONE");
		if ((padapter->bSurpriseRemoved == 0)&&_FAIL == rtw_pwr_wakeup(padapter))
		if (padapter->bSurpriseRemoved == 0 &&
		rtw_pwr_wakeup(padapter) == _FAIL)
		return -EFAULT;
		}
		else {
		} else
		return -EINVAL;
		}

		return 0;
		}