drivers/staging/rtl8723bs/core/rtw_pwrctrl.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /******************************************************************************
   3  *
   4  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   5  *
   6  ******************************************************************************/
   7 #define _RTW_PWRCTRL_C_
   8
   9 #include <drv_types.h>
  10 #include <rtw_debug.h>
  11 #include <hal_data.h>
  12 #include <linux/jiffies.h>
  13
  14
  15 void _ips_enter(struct adapter *padapter)
  16 {
  17         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
  18
  19         pwrpriv->bips_processing = true;
  20
  21         /*  syn ips_mode with request */
  22         pwrpriv->ips_mode = pwrpriv->ips_mode_req;
  23
  24         pwrpriv->ips_enter_cnts++;
  25         DBG_871X("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);
  26
  27         if (rf_off == pwrpriv->change_rfpwrstate) {
  28                 pwrpriv->bpower_saving = true;
  29                 DBG_871X("nolinked power save enter\n");
  30
  31                 if (pwrpriv->ips_mode == IPS_LEVEL_2)
  32                         pwrpriv->bkeepfwalive = true;
  33
  34                 rtw_ips_pwr_down(padapter);
  35                 pwrpriv->rf_pwrstate = rf_off;
  36         }
  37         pwrpriv->bips_processing = false;
  38
  39 }
  40
  41 void ips_enter(struct adapter *padapter)
  42 {
  43         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
  44
  45
  46         hal_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
  47
  48         mutex_lock(&pwrpriv->lock);
  49         _ips_enter(padapter);
  50         mutex_unlock(&pwrpriv->lock);
  51 }
  52
  53 int _ips_leave(struct adapter *padapter)
  54 {
  55         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
  56         int result = _SUCCESS;
  57
  58         if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) {
  59                 pwrpriv->bips_processing = true;
  60                 pwrpriv->change_rfpwrstate = rf_on;
  61                 pwrpriv->ips_leave_cnts++;
  62                 DBG_871X("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);
  63
  64                 result = rtw_ips_pwr_up(padapter);
  65                 if (result == _SUCCESS) {
  66                         pwrpriv->rf_pwrstate = rf_on;
  67                 }
  68                 DBG_871X("nolinked power save leave\n");
  69
  70                 DBG_871X("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
  71                 pwrpriv->bips_processing = false;
  72
  73                 pwrpriv->bkeepfwalive = false;
  74                 pwrpriv->bpower_saving = false;
  75         }
  76
  77         return result;
  78 }
  79
  80 int ips_leave(struct adapter *padapter)
  81 {
  82         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
  83         int ret;
  84
  85         if (!is_primary_adapter(padapter))
  86                 return _SUCCESS;
  87
  88         mutex_lock(&pwrpriv->lock);
  89         ret = _ips_leave(padapter);
  90         mutex_unlock(&pwrpriv->lock);
  91
  92         if (_SUCCESS == ret)
  93                 hal_btcoex_IpsNotify(padapter, IPS_NONE);
  94
  95         return ret;
  96 }
  97
  98 static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
  99 {
 100         struct adapter *buddy = adapter->pbuddy_adapter;
 101         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
 102         struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
 103
 104         bool ret = false;
 105
 106         if (adapter_to_pwrctl(adapter)->bpower_saving) {
 107                 /* DBG_871X("%s: already in LPS or IPS mode\n", __func__); */
 108                 goto exit;
 109         }
 110
 111         if (time_before(jiffies, adapter_to_pwrctl(adapter)->ips_deny_time)) {
 112                 /* DBG_871X("%s ips_deny_time\n", __func__); */
 113                 goto exit;
 114         }
 115
 116         if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
 117                 || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
 118                 || check_fwstate(pmlmepriv, WIFI_AP_STATE)
 119                 || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
 120         )
 121                 goto exit;
 122
 123         /* consider buddy, if exist */
 124         if (buddy) {
 125                 struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
 126
 127                 if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
 128                         || check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
 129                         || check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
 130                         || check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
 131                 )
 132                         goto exit;
 133         }
 134
 135         if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
 136                 pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
 137                 DBG_871X_LEVEL(_drv_always_, "There are some pkts to transmit\n");
 138                 DBG_871X_LEVEL(_drv_always_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
 139                         pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
 140                 goto exit;
 141         }
 142
 143         ret = true;
 144
 145 exit:
 146         return ret;
 147 }
 148
 149
 150 /*
 151  * ATTENTION:
 152  *rtw_ps_processor() doesn't handle LPS.
 153  */
 154 void rtw_ps_processor(struct adapter *padapter)
 155 {
 156         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 157         struct dvobj_priv *psdpriv = padapter->dvobj;
 158         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
 159         u32 ps_deny = 0;
 160
 161         mutex_lock(&adapter_to_pwrctl(padapter)->lock);
 162         ps_deny = rtw_ps_deny_get(padapter);
 163         mutex_unlock(&adapter_to_pwrctl(padapter)->lock);
 164         if (ps_deny != 0) {
 165                 DBG_871X(FUNC_ADPT_FMT ": ps_deny = 0x%08X, skip power save!\n",
 166                         FUNC_ADPT_ARG(padapter), ps_deny);
 167                 goto exit;
 168         }
 169
 170         if (pwrpriv->bInSuspend) {/* system suspend or autosuspend */
 171                 pdbgpriv->dbg_ps_insuspend_cnt++;
 172                 DBG_871X("%s, pwrpriv->bInSuspend == true ignore this process\n", __func__);
 173                 return;
 174         }
 175
 176         pwrpriv->ps_processing = true;
 177
 178         if (pwrpriv->ips_mode_req == IPS_NONE)
 179                 goto exit;
 180
 181         if (!rtw_pwr_unassociated_idle(padapter))
 182                 goto exit;
 183
 184         if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0)) {
 185                 DBG_871X("==>%s\n", __func__);
 186                 pwrpriv->change_rfpwrstate = rf_off;
 187                 {
 188                         ips_enter(padapter);
 189                 }
 190         }
 191 exit:
 192         pwrpriv->ps_processing = false;
 193 }
 194
 195 static void pwr_state_check_handler(struct timer_list *t)
 196 {
 197         struct pwrctrl_priv *pwrctrlpriv =
 198                 from_timer(pwrctrlpriv, t, pwr_state_check_timer);
 199         struct adapter *padapter = pwrctrlpriv->adapter;
 200
 201         rtw_ps_cmd(padapter);
 202 }
 203
 204 void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets)
 205 {
 206         static unsigned long start_time;
 207         static u32 xmit_cnt;
 208         u8 bLeaveLPS = false;
 209         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 210
 211
 212
 213         if (tx) { /* from tx */
 214                 xmit_cnt += tx_packets;
 215
 216                 if (start_time == 0)
 217                         start_time = jiffies;
 218
 219                 if (jiffies_to_msecs(jiffies - start_time) > 2000) { /*  2 sec == watch dog timer */
 220                         if (xmit_cnt > 8) {
 221                                 if (adapter_to_pwrctl(padapter)->bLeisurePs
 222                                     && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
 223                                     && !(hal_btcoex_IsBtControlLps(padapter))) {
 224                                         DBG_871X("leave lps via Tx = %d\n", xmit_cnt);
 225                                         bLeaveLPS = true;
 226                                 }
 227                         }
 228
 229                         start_time = jiffies;
 230                         xmit_cnt = 0;
 231                 }
 232
 233         } else { /*  from rx path */
 234                 if (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4/*2*/) {
 235                         if (adapter_to_pwrctl(padapter)->bLeisurePs
 236                             && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
 237                             && !(hal_btcoex_IsBtControlLps(padapter))) {
 238                                 DBG_871X("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
 239                                 bLeaveLPS = true;
 240                         }
 241                 }
 242         }
 243
 244         if (bLeaveLPS)
 245                 /* DBG_871X("leave lps via %s, Tx = %d, Rx = %d\n", tx?"Tx":"Rx", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
 246                 /* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1); */
 247                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, tx?0:1);
 248 }
 249
 250 /*
 251  * Description:
 252  *This function MUST be called under power lock protect
 253  *
 254  * Parameters
 255  *padapter
 256  *pslv                  power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
 257  *
 258  */
 259 void rtw_set_rpwm(struct adapter *padapter, u8 pslv)
 260 {
 261         u8 rpwm;
 262         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 263         u8 cpwm_orig;
 264
 265         pslv = PS_STATE(pslv);
 266
 267         if (pwrpriv->brpwmtimeout) {
 268                 DBG_871X("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __func__, pslv);
 269         } else {
 270                 if ((pwrpriv->rpwm == pslv)
 271                         || ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2))) {
 272                         RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
 273                                 ("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
 274                         return;
 275                 }
 276         }
 277
 278         if ((padapter->bSurpriseRemoved) || !(padapter->hw_init_completed)) {
 279                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
 280                                  ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
 281                                   __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
 282
 283                 pwrpriv->cpwm = PS_STATE_S4;
 284
 285                 return;
 286         }
 287
 288         if (padapter->bDriverStopped) {
 289                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
 290                                  ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
 291
 292                 if (pslv < PS_STATE_S2) {
 293                         RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
 294                                          ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
 295                         return;
 296                 }
 297         }
 298
 299         rpwm = pslv | pwrpriv->tog;
 300         /*  only when from PS_STATE S0/S1 to S2 and higher needs ACK */
 301         if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2))
 302                 rpwm |= PS_ACK;
 303         RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 304                          ("rtw_set_rpwm: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
 305
 306         pwrpriv->rpwm = pslv;
 307
 308         cpwm_orig = 0;
 309         if (rpwm & PS_ACK)
 310                 rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
 311
 312         if (rpwm & PS_ACK)
 313                 _set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS);
 314         rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
 315
 316         pwrpriv->tog += 0x80;
 317
 318         /*  No LPS 32K, No Ack */
 319         if (rpwm & PS_ACK) {
 320                 unsigned long start_time;
 321                 u8 cpwm_now;
 322                 u8 poll_cnt = 0;
 323
 324                 start_time = jiffies;
 325
 326                 /*  polling cpwm */
 327                 do {
 328                         mdelay(1);
 329                         poll_cnt++;
 330                         rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now);
 331                         if ((cpwm_orig ^ cpwm_now) & 0x80) {
 332                                 pwrpriv->cpwm = PS_STATE_S4;
 333                                 pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE;
 334                                 break;
 335                         }
 336
 337                         if (jiffies_to_msecs(jiffies - start_time) > LPS_RPWM_WAIT_MS) {
 338                                 DBG_871X("%s: polling cpwm timeout! poll_cnt =%d, cpwm_orig =%02x, cpwm_now =%02x\n", __func__, poll_cnt, cpwm_orig, cpwm_now);
 339                                 _set_timer(&pwrpriv->pwr_rpwm_timer, 1);
 340                                 break;
 341                         }
 342                 } while (1);
 343         } else
 344                 pwrpriv->cpwm = pslv;
 345 }
 346
 347 static u8 PS_RDY_CHECK(struct adapter *padapter)
 348 {
 349         unsigned long curr_time, delta_time;
 350         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 351         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 352
 353 #if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
 354         if (pwrpriv->bInSuspend && pwrpriv->wowlan_mode)
 355                 return true;
 356         else if (pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode)
 357                 return true;
 358         else if (pwrpriv->bInSuspend)
 359                 return false;
 360 #else
 361         if (pwrpriv->bInSuspend)
 362                 return false;
 363 #endif
 364
 365         curr_time = jiffies;
 366
 367         delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp;
 368
 369         if (delta_time < LPS_DELAY_TIME)
 370                 return false;
 371
 372         if (check_fwstate(pmlmepriv, WIFI_SITE_MONITOR)
 373                 || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
 374                 || check_fwstate(pmlmepriv, WIFI_AP_STATE)
 375                 || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
 376                 || rtw_is_scan_deny(padapter)
 377         )
 378                 return false;
 379
 380         if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && !(padapter->securitypriv.binstallGrpkey)) {
 381                 DBG_871X("Group handshake still in progress !!!\n");
 382                 return false;
 383         }
 384
 385         if (!rtw_cfg80211_pwr_mgmt(padapter))
 386                 return false;
 387
 388         return true;
 389 }
 390
 391 void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg)
 392 {
 393         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 394 #if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
 395         struct debug_priv *pdbgpriv = &padapter->dvobj->drv_dbg;
 396 #endif
 397
 398         RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 399                          ("%s: PowerMode =%d Smart_PS =%d\n",
 400                           __func__, ps_mode, smart_ps));
 401
 402         if (ps_mode > PM_Card_Disable) {
 403                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
 404                 return;
 405         }
 406
 407         if (pwrpriv->pwr_mode == ps_mode)
 408                 if (PS_MODE_ACTIVE == ps_mode)
 409                         return;
 410
 411
 412         mutex_lock(&pwrpriv->lock);
 413
 414         /* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
 415         if (ps_mode == PS_MODE_ACTIVE) {
 416                 if (!(hal_btcoex_IsBtControlLps(padapter))
 417                                 || (hal_btcoex_IsBtControlLps(padapter)
 418                                         && !(hal_btcoex_IsLpsOn(padapter)))) {
 419                         DBG_871X(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n",
 420                                 FUNC_ADPT_ARG(padapter), msg);
 421
 422                         pwrpriv->pwr_mode = ps_mode;
 423                         rtw_set_rpwm(padapter, PS_STATE_S4);
 424
 425 #if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
 426                         if (pwrpriv->wowlan_mode || pwrpriv->wowlan_ap_mode) {
 427                                 unsigned long start_time;
 428                                 u32 delay_ms;
 429                                 u8 val8;
 430                                 delay_ms = 20;
 431                                 start_time = jiffies;
 432                                 do {
 433                                         rtw_hal_get_hwreg(padapter, HW_VAR_SYS_CLKR, &val8);
 434                                         if (!(val8 & BIT(4))) { /* 0x08 bit4 = 1 --> in 32k, bit4 = 0 --> leave 32k */
 435                                                 pwrpriv->cpwm = PS_STATE_S4;
 436                                                 break;
 437                                         }
 438                                         if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
 439                                                 DBG_871X("%s: Wait for FW 32K leave more than %u ms!!!\n",
 440                                                                 __func__, delay_ms);
 441                                                 pdbgpriv->dbg_wow_leave_ps_fail_cnt++;
 442                                                 break;
 443                                         }
 444                                         msleep(1);
 445                                 } while (1);
 446                         }
 447 #endif
 448                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
 449                         pwrpriv->bFwCurrentInPSMode = false;
 450
 451                         hal_btcoex_LpsNotify(padapter, ps_mode);
 452                 }
 453         } else {
 454                 if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE))
 455                         || ((hal_btcoex_IsBtControlLps(padapter))
 456                                 && (hal_btcoex_IsLpsOn(padapter)))
 457                         ) {
 458                         u8 pslv;
 459
 460                         DBG_871X(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n",
 461                                 FUNC_ADPT_ARG(padapter), msg);
 462
 463                         hal_btcoex_LpsNotify(padapter, ps_mode);
 464
 465                         pwrpriv->bFwCurrentInPSMode = true;
 466                         pwrpriv->pwr_mode = ps_mode;
 467                         pwrpriv->smart_ps = smart_ps;
 468                         pwrpriv->bcn_ant_mode = bcn_ant_mode;
 469                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
 470
 471                         pslv = PS_STATE_S2;
 472                         if (pwrpriv->alives == 0)
 473                                 pslv = PS_STATE_S0;
 474
 475                         if (!(hal_btcoex_IsBtDisabled(padapter))
 476                                 && (hal_btcoex_IsBtControlLps(padapter))) {
 477                                 u8 val8;
 478
 479                                 val8 = hal_btcoex_LpsVal(padapter);
 480                                 if (val8 & BIT(4))
 481                                         pslv = PS_STATE_S2;
 482                         }
 483
 484                         rtw_set_rpwm(padapter, pslv);
 485                 }
 486         }
 487
 488         mutex_unlock(&pwrpriv->lock);
 489 }
 490
 491 /*
 492  * Return:
 493  *0:    Leave OK
 494  *-1:   Timeout
 495  *-2:   Other error
 496  */
 497 s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
 498 {
 499         unsigned long start_time;
 500         u8 bAwake = false;
 501         s32 err = 0;
 502
 503
 504         start_time = jiffies;
 505         while (1) {
 506                 rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
 507                 if (bAwake)
 508                         break;
 509
 510                 if (padapter->bSurpriseRemoved) {
 511                         err = -2;
 512                         DBG_871X("%s: device surprise removed!!\n", __func__);
 513                         break;
 514                 }
 515
 516                 if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
 517                         err = -1;
 518                         DBG_871X("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
 519                         break;
 520                 }
 521                 msleep(1);
 522         }
 523
 524         return err;
 525 }
 526
 527 /*  */
 528 /*      Description: */
 529 /*              Enter the leisure power save mode. */
 530 /*  */
 531 void LPS_Enter(struct adapter *padapter, const char *msg)
 532 {
 533         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
 534         struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
 535         int n_assoc_iface = 0;
 536         char buf[32] = {0};
 537
 538         if (hal_btcoex_IsBtControlLps(padapter))
 539                 return;
 540
 541         /* Skip lps enter request if number of assocated adapters is not 1 */
 542         if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
 543                 n_assoc_iface++;
 544         if (n_assoc_iface != 1)
 545                 return;
 546
 547         /* Skip lps enter request for adapter not port0 */
 548         if (get_iface_type(padapter) != IFACE_PORT0)
 549                 return;
 550
 551         if (!PS_RDY_CHECK(dvobj->padapters))
 552                 return;
 553
 554         if (pwrpriv->bLeisurePs) {
 555                 /*  Idle for a while if we connect to AP a while ago. */
 556                 if (pwrpriv->LpsIdleCount >= 2) { /*   4 Sec */
 557                         if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
 558                                 sprintf(buf, "WIFI-%s", msg);
 559                                 pwrpriv->bpower_saving = true;
 560                                 rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, padapter->registrypriv.smart_ps, 0, buf);
 561                         }
 562                 } else
 563                         pwrpriv->LpsIdleCount++;
 564         }
 565
 566 /*      DBG_871X("-LeisurePSEnter\n"); */
 567 }
 568
 569 /*  */
 570 /*      Description: */
 571 /*              Leave the leisure power save mode. */
 572 /*  */
 573 void LPS_Leave(struct adapter *padapter, const char *msg)
 574 {
 575 #define LPS_LEAVE_TIMEOUT_MS 100
 576
 577         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
 578         struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
 579         char buf[32] = {0};
 580
 581 /*      DBG_871X("+LeisurePSLeave\n"); */
 582
 583         if (hal_btcoex_IsBtControlLps(padapter))
 584                 return;
 585
 586         if (pwrpriv->bLeisurePs) {
 587                 if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
 588                         sprintf(buf, "WIFI-%s", msg);
 589                         rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, buf);
 590
 591                         if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
 592                                 LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
 593                 }
 594         }
 595
 596         pwrpriv->bpower_saving = false;
 597 /*      DBG_871X("-LeisurePSLeave\n"); */
 598
 599 }
 600
 601 void LeaveAllPowerSaveModeDirect(struct adapter *Adapter)
 602 {
 603         struct adapter *pri_padapter = GET_PRIMARY_ADAPTER(Adapter);
 604         struct mlme_priv *pmlmepriv = &(Adapter->mlmepriv);
 605         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter);
 606
 607         DBG_871X("%s.....\n", __func__);
 608
 609         if (Adapter->bSurpriseRemoved) {
 610                 DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
 611                         FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved);
 612                 return;
 613         }
 614
 615         if (check_fwstate(pmlmepriv, _FW_LINKED)) { /* connect */
 616
 617                 if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
 618                         DBG_871X("%s: Driver Already Leave LPS\n", __func__);
 619                         return;
 620                 }
 621
 622                 mutex_lock(&pwrpriv->lock);
 623
 624                 rtw_set_rpwm(Adapter, PS_STATE_S4);
 625
 626                 mutex_unlock(&pwrpriv->lock);
 627
 628                 rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
 629         } else {
 630                 if (pwrpriv->rf_pwrstate == rf_off)
 631                         if (!ips_leave(pri_padapter))
 632                                 DBG_871X("======> ips_leave fail.............\n");
 633         }
 634 }
 635
 636 /*  */
 637 /*  Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */
 638 /*  Move code to function by tynli. 2010.03.26. */
 639 /*  */
 640 void LeaveAllPowerSaveMode(struct adapter *Adapter)
 641 {
 642         struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
 643         u8 enqueue = 0;
 644         int n_assoc_iface = 0;
 645
 646         if (!Adapter->bup) {
 647                 DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n",
 648                         FUNC_ADPT_ARG(Adapter), Adapter->bup);
 649                 return;
 650         }
 651
 652         if (Adapter->bSurpriseRemoved) {
 653                 DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
 654                         FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved);
 655                 return;
 656         }
 657
 658         if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
 659                 n_assoc_iface++;
 660
 661         if (n_assoc_iface) { /* connect */
 662                 enqueue = 1;
 663
 664                 rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
 665
 666                 LPS_Leave_check(Adapter);
 667         } else {
 668                 if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) {
 669                         if (!ips_leave(Adapter))
 670                                 DBG_871X("======> ips_leave fail.............\n");
 671                 }
 672         }
 673 }
 674
 675 void LPS_Leave_check(
 676         struct adapter *padapter)
 677 {
 678         struct pwrctrl_priv *pwrpriv;
 679         unsigned long   start_time;
 680         u8 bReady;
 681
 682         pwrpriv = adapter_to_pwrctl(padapter);
 683
 684         bReady = false;
 685         start_time = jiffies;
 686
 687         cond_resched();
 688
 689         while (1) {
 690                 mutex_lock(&pwrpriv->lock);
 691
 692                 if (padapter->bSurpriseRemoved
 693                         || !(padapter->hw_init_completed)
 694                         || (pwrpriv->pwr_mode == PS_MODE_ACTIVE))
 695                         bReady = true;
 696
 697                 mutex_unlock(&pwrpriv->lock);
 698
 699                 if (bReady)
 700                         break;
 701
 702                 if (jiffies_to_msecs(jiffies - start_time) > 100) {
 703                         DBG_871X("Wait for cpwm event  than 100 ms!!!\n");
 704                         break;
 705                 }
 706                 msleep(1);
 707         }
 708 }
 709
 710 /*
 711  * Caller:ISR handler...
 712  *
 713  * This will be called when CPWM interrupt is up.
 714  *
 715  * using to update cpwn of drv; and drv willl make a decision to up or down pwr level
 716  */
 717 void cpwm_int_hdl(
 718         struct adapter *padapter,
 719         struct reportpwrstate_parm *preportpwrstate)
 720 {
 721         struct pwrctrl_priv *pwrpriv;
 722
 723         pwrpriv = adapter_to_pwrctl(padapter);
 724
 725         mutex_lock(&pwrpriv->lock);
 726
 727         if (pwrpriv->rpwm < PS_STATE_S2) {
 728                 DBG_871X("%s: Redundant CPWM Int. RPWM = 0x%02X CPWM = 0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
 729                 goto exit;
 730         }
 731
 732         pwrpriv->cpwm = PS_STATE(preportpwrstate->state);
 733         pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE;
 734
 735         if (pwrpriv->cpwm >= PS_STATE_S2) {
 736                 if (pwrpriv->alives & CMD_ALIVE)
 737                         complete(&padapter->cmdpriv.cmd_queue_comp);
 738
 739                 if (pwrpriv->alives & XMIT_ALIVE)
 740                         complete(&padapter->xmitpriv.xmit_comp);
 741         }
 742
 743 exit:
 744         mutex_unlock(&pwrpriv->lock);
 745
 746         RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 747                          ("cpwm_int_hdl: cpwm = 0x%02x\n", pwrpriv->cpwm));
 748 }
 749
 750 static void cpwm_event_callback(struct work_struct *work)
 751 {
 752         struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event);
 753         struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
 754         struct adapter *adapter = dvobj->if1;
 755         struct reportpwrstate_parm report;
 756
 757         /* DBG_871X("%s\n", __func__); */
 758
 759         report.state = PS_STATE_S2;
 760         cpwm_int_hdl(adapter, &report);
 761 }
 762
 763 static void rpwmtimeout_workitem_callback(struct work_struct *work)
 764 {
 765         struct adapter *padapter;
 766         struct dvobj_priv *dvobj;
 767         struct pwrctrl_priv *pwrpriv;
 768
 769
 770         pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi);
 771         dvobj = pwrctl_to_dvobj(pwrpriv);
 772         padapter = dvobj->if1;
 773 /*      DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); */
 774
 775         mutex_lock(&pwrpriv->lock);
 776         if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
 777                 DBG_871X("%s: rpwm = 0x%02X cpwm = 0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
 778                 goto exit;
 779         }
 780         mutex_unlock(&pwrpriv->lock);
 781
 782         if (rtw_read8(padapter, 0x100) != 0xEA) {
 783                 struct reportpwrstate_parm report;
 784
 785                 report.state = PS_STATE_S2;
 786                 DBG_871X("\n%s: FW already leave 32K!\n\n", __func__);
 787                 cpwm_int_hdl(padapter, &report);
 788
 789                 return;
 790         }
 791
 792         mutex_lock(&pwrpriv->lock);
 793
 794         if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
 795                 DBG_871X("%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
 796                 goto exit;
 797         }
 798         pwrpriv->brpwmtimeout = true;
 799         rtw_set_rpwm(padapter, pwrpriv->rpwm);
 800         pwrpriv->brpwmtimeout = false;
 801
 802 exit:
 803         mutex_unlock(&pwrpriv->lock);
 804 }
 805
 806 /*
 807  * This function is a timer handler, can't do any IO in it.
 808  */
 809 static void pwr_rpwm_timeout_handler(struct timer_list *t)
 810 {
 811         struct pwrctrl_priv *pwrpriv = from_timer(pwrpriv, t, pwr_rpwm_timer);
 812
 813         DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
 814
 815         if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
 816                 DBG_871X("+%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
 817                 return;
 818         }
 819
 820         _set_workitem(&pwrpriv->rpwmtimeoutwi);
 821 }
 822
 823 static inline void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
 824 {
 825         pwrctrl->alives |= tag;
 826 }
 827
 828 static inline void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
 829 {
 830         pwrctrl->alives &= ~tag;
 831 }
 832
 833
 834 /*
 835  * Description:
 836  *Check if the fw_pwrstate is okay for I/O.
 837  *If not (cpwm is less than S2), then the sub-routine
 838  *will raise the cpwm to be greater than or equal to S2.
 839  *
 840  *Calling Context: Passive
 841  *
 842  *Constraint:
 843  *      1. this function will request pwrctrl->lock
 844  *
 845  * Return Value:
 846  *_SUCCESS      hardware is ready for I/O
 847  *_FAIL         can't I/O right now
 848  */
 849 s32 rtw_register_task_alive(struct adapter *padapter, u32 task)
 850 {
 851         s32 res;
 852         struct pwrctrl_priv *pwrctrl;
 853         u8 pslv;
 854
 855         res = _SUCCESS;
 856         pwrctrl = adapter_to_pwrctl(padapter);
 857         pslv = PS_STATE_S2;
 858
 859         mutex_lock(&pwrctrl->lock);
 860
 861         register_task_alive(pwrctrl, task);
 862
 863         if (pwrctrl->bFwCurrentInPSMode) {
 864                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 865                                  ("%s: task = 0x%x cpwm = 0x%02x alives = 0x%08x\n",
 866                                   __func__, task, pwrctrl->cpwm, pwrctrl->alives));
 867
 868                 if (pwrctrl->cpwm < pslv) {
 869                         if (pwrctrl->cpwm < PS_STATE_S2)
 870                                 res = _FAIL;
 871                         if (pwrctrl->rpwm < pslv)
 872                                 rtw_set_rpwm(padapter, pslv);
 873                 }
 874         }
 875
 876         mutex_unlock(&pwrctrl->lock);
 877
 878         if (_FAIL == res)
 879                 if (pwrctrl->cpwm >= PS_STATE_S2)
 880                         res = _SUCCESS;
 881
 882         return res;
 883 }
 884
 885 /*
 886  * Description:
 887  *If task is done, call this func. to power down firmware again.
 888  *
 889  *Constraint:
 890  *      1. this function will request pwrctrl->lock
 891  *
 892  * Return Value:
 893  *none
 894  */
 895 void rtw_unregister_task_alive(struct adapter *padapter, u32 task)
 896 {
 897         struct pwrctrl_priv *pwrctrl;
 898         u8 pslv;
 899
 900         pwrctrl = adapter_to_pwrctl(padapter);
 901         pslv = PS_STATE_S0;
 902
 903         if (!(hal_btcoex_IsBtDisabled(padapter))
 904             && hal_btcoex_IsBtControlLps(padapter)) {
 905                 u8 val8;
 906
 907                 val8 = hal_btcoex_LpsVal(padapter);
 908                 if (val8 & BIT(4))
 909                         pslv = PS_STATE_S2;
 910         }
 911
 912         mutex_lock(&pwrctrl->lock);
 913
 914         unregister_task_alive(pwrctrl, task);
 915
 916         if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
 917             && pwrctrl->bFwCurrentInPSMode) {
 918                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 919                                  ("%s: cpwm = 0x%02x alives = 0x%08x\n",
 920                                   __func__, pwrctrl->cpwm, pwrctrl->alives));
 921
 922                 if (pwrctrl->cpwm > pslv)
 923                         if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
 924                                 rtw_set_rpwm(padapter, pslv);
 925
 926         }
 927
 928         mutex_unlock(&pwrctrl->lock);
 929 }
 930
 931 /*
 932  * Caller: rtw_xmit_thread
 933  *
 934  * Check if the fw_pwrstate is okay for xmit.
 935  * If not (cpwm is less than S3), then the sub-routine
 936  * will raise the cpwm to be greater than or equal to S3.
 937  *
 938  * Calling Context: Passive
 939  *
 940  * Return Value:
 941  * _SUCCESS     rtw_xmit_thread can write fifo/txcmd afterwards.
 942  * _FAIL                rtw_xmit_thread can not do anything.
 943  */
 944 s32 rtw_register_tx_alive(struct adapter *padapter)
 945 {
 946         s32 res;
 947         struct pwrctrl_priv *pwrctrl;
 948         u8 pslv;
 949
 950         res = _SUCCESS;
 951         pwrctrl = adapter_to_pwrctl(padapter);
 952         pslv = PS_STATE_S2;
 953
 954         mutex_lock(&pwrctrl->lock);
 955
 956         register_task_alive(pwrctrl, XMIT_ALIVE);
 957
 958         if (pwrctrl->bFwCurrentInPSMode) {
 959                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 960                                  ("rtw_register_tx_alive: cpwm = 0x%02x alives = 0x%08x\n",
 961                                   pwrctrl->cpwm, pwrctrl->alives));
 962
 963                 if (pwrctrl->cpwm < pslv) {
 964                         if (pwrctrl->cpwm < PS_STATE_S2)
 965                                 res = _FAIL;
 966                         if (pwrctrl->rpwm < pslv)
 967                                 rtw_set_rpwm(padapter, pslv);
 968                 }
 969         }
 970
 971         mutex_unlock(&pwrctrl->lock);
 972
 973         if (_FAIL == res)
 974                 if (pwrctrl->cpwm >= PS_STATE_S2)
 975                         res = _SUCCESS;
 976
 977         return res;
 978 }
 979
 980 /*
 981  * Caller: rtw_cmd_thread
 982  *
 983  * Check if the fw_pwrstate is okay for issuing cmd.
 984  * If not (cpwm should be is less than S2), then the sub-routine
 985  * will raise the cpwm to be greater than or equal to S2.
 986  *
 987  * Calling Context: Passive
 988  *
 989  * Return Value:
 990  *_SUCCESS      rtw_cmd_thread can issue cmds to firmware afterwards.
 991  *_FAIL         rtw_cmd_thread can not do anything.
 992  */
 993 s32 rtw_register_cmd_alive(struct adapter *padapter)
 994 {
 995         s32 res;
 996         struct pwrctrl_priv *pwrctrl;
 997         u8 pslv;
 998
 999         res = _SUCCESS;
1000         pwrctrl = adapter_to_pwrctl(padapter);
1001         pslv = PS_STATE_S2;
1002
1003         mutex_lock(&pwrctrl->lock);
1004
1005         register_task_alive(pwrctrl, CMD_ALIVE);
1006
1007         if (pwrctrl->bFwCurrentInPSMode) {
1008                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
1009                                  ("rtw_register_cmd_alive: cpwm = 0x%02x alives = 0x%08x\n",
1010                                   pwrctrl->cpwm, pwrctrl->alives));
1011
1012                 if (pwrctrl->cpwm < pslv) {
1013                         if (pwrctrl->cpwm < PS_STATE_S2)
1014                                 res = _FAIL;
1015                         if (pwrctrl->rpwm < pslv)
1016                                 rtw_set_rpwm(padapter, pslv);
1017                 }
1018         }
1019
1020         mutex_unlock(&pwrctrl->lock);
1021
1022         if (_FAIL == res)
1023                 if (pwrctrl->cpwm >= PS_STATE_S2)
1024                         res = _SUCCESS;
1025
1026         return res;
1027 }
1028
1029 /*
1030  * Caller: ISR
1031  *
1032  * If ISR's txdone,
1033  * No more pkts for TX,
1034  * Then driver shall call this fun. to power down firmware again.
1035  */
1036 void rtw_unregister_tx_alive(struct adapter *padapter)
1037 {
1038         struct pwrctrl_priv *pwrctrl;
1039         u8 pslv;
1040
1041         pwrctrl = adapter_to_pwrctl(padapter);
1042         pslv = PS_STATE_S0;
1043
1044         if (!(hal_btcoex_IsBtDisabled(padapter))
1045                 && hal_btcoex_IsBtControlLps(padapter)) {
1046                 u8 val8;
1047
1048                 val8 = hal_btcoex_LpsVal(padapter);
1049                 if (val8 & BIT(4))
1050                         pslv = PS_STATE_S2;
1051         }
1052
1053         mutex_lock(&pwrctrl->lock);
1054
1055         unregister_task_alive(pwrctrl, XMIT_ALIVE);
1056
1057         if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
1058                 && pwrctrl->bFwCurrentInPSMode) {
1059                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
1060                                  ("%s: cpwm = 0x%02x alives = 0x%08x\n",
1061                                   __func__, pwrctrl->cpwm, pwrctrl->alives));
1062
1063                 if (pwrctrl->cpwm > pslv)
1064                         if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
1065                                 rtw_set_rpwm(padapter, pslv);
1066         }
1067
1068         mutex_unlock(&pwrctrl->lock);
1069 }
1070
1071 /*
1072  * Caller: ISR
1073  *
1074  * If all commands have been done,
1075  * and no more command to do,
1076  * then driver shall call this fun. to power down firmware again.
1077  */
1078 void rtw_unregister_cmd_alive(struct adapter *padapter)
1079 {
1080         struct pwrctrl_priv *pwrctrl;
1081         u8 pslv;
1082
1083         pwrctrl = adapter_to_pwrctl(padapter);
1084         pslv = PS_STATE_S0;
1085
1086         if (!(hal_btcoex_IsBtDisabled(padapter))
1087             && hal_btcoex_IsBtControlLps(padapter)) {
1088                 u8 val8;
1089
1090                 val8 = hal_btcoex_LpsVal(padapter);
1091                 if (val8 & BIT(4))
1092                         pslv = PS_STATE_S2;
1093         }
1094
1095         mutex_lock(&pwrctrl->lock);
1096
1097         unregister_task_alive(pwrctrl, CMD_ALIVE);
1098
1099         if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
1100             && pwrctrl->bFwCurrentInPSMode) {
1101                 RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
1102                                  ("%s: cpwm = 0x%02x alives = 0x%08x\n",
1103                                   __func__, pwrctrl->cpwm, pwrctrl->alives));
1104
1105                 if (pwrctrl->cpwm > pslv) {
1106                         if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
1107                                 rtw_set_rpwm(padapter, pslv);
1108                 }
1109         }
1110
1111         mutex_unlock(&pwrctrl->lock);
1112 }
1113
1114 void rtw_init_pwrctrl_priv(struct adapter *padapter)
1115 {
1116         struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
1117
1118         mutex_init(&pwrctrlpriv->lock);
1119         pwrctrlpriv->rf_pwrstate = rf_on;
1120         pwrctrlpriv->ips_enter_cnts = 0;
1121         pwrctrlpriv->ips_leave_cnts = 0;
1122         pwrctrlpriv->bips_processing = false;
1123
1124         pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
1125         pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
1126
1127         pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
1128         pwrctrlpriv->pwr_state_check_cnts = 0;
1129         pwrctrlpriv->bInternalAutoSuspend = false;
1130         pwrctrlpriv->bInSuspend = false;
1131         pwrctrlpriv->bkeepfwalive = false;
1132
1133         pwrctrlpriv->LpsIdleCount = 0;
1134         pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
1135         pwrctrlpriv->bLeisurePs = pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
1136
1137         pwrctrlpriv->bFwCurrentInPSMode = false;
1138
1139         pwrctrlpriv->rpwm = 0;
1140         pwrctrlpriv->cpwm = PS_STATE_S4;
1141
1142         pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
1143         pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
1144         pwrctrlpriv->bcn_ant_mode = 0;
1145         pwrctrlpriv->dtim = 0;
1146
1147         pwrctrlpriv->tog = 0x80;
1148
1149         rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm));
1150
1151         _init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL);
1152
1153         pwrctrlpriv->brpwmtimeout = false;
1154         pwrctrlpriv->adapter = padapter;
1155         _init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
1156         timer_setup(&pwrctrlpriv->pwr_rpwm_timer, pwr_rpwm_timeout_handler, 0);
1157         timer_setup(&pwrctrlpriv->pwr_state_check_timer,
1158                     pwr_state_check_handler, 0);
1159
1160         pwrctrlpriv->wowlan_mode = false;
1161         pwrctrlpriv->wowlan_ap_mode = false;
1162
1163 #ifdef CONFIG_PNO_SUPPORT
1164         pwrctrlpriv->pno_inited = false;
1165         pwrctrlpriv->pnlo_info = NULL;
1166         pwrctrlpriv->pscan_info = NULL;
1167         pwrctrlpriv->pno_ssid_list = NULL;
1168         pwrctrlpriv->pno_in_resume = true;
1169 #endif
1170 }
1171
1172
1173 void rtw_free_pwrctrl_priv(struct adapter *adapter)
1174 {
1175 #ifdef CONFIG_PNO_SUPPORT
1176         if (pwrctrlpriv->pnlo_info != NULL)
1177                 printk("****** pnlo_info memory leak********\n");
1178
1179         if (pwrctrlpriv->pscan_info != NULL)
1180                 printk("****** pscan_info memory leak********\n");
1181
1182         if (pwrctrlpriv->pno_ssid_list != NULL)
1183                 printk("****** pno_ssid_list memory leak********\n");
1184 #endif
1185 }
1186
1187 inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
1188 {
1189         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
1190         pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ms);
1191 }
1192
1193 /*
1194 * rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
1195 * @adapter: pointer to struct adapter structure
1196 * @ips_deffer_ms: the ms will prevent from falling into IPS after wakeup
1197 * Return _SUCCESS or _FAIL
1198 */
1199
1200 int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller)
1201 {
1202         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
1203         struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
1204         struct mlme_priv *pmlmepriv;
1205         int ret = _SUCCESS;
1206         unsigned long start = jiffies;
1207         unsigned long deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
1208
1209         /* for LPS */
1210         LeaveAllPowerSaveMode(padapter);
1211
1212         /* IPS still bound with primary adapter */
1213         padapter = GET_PRIMARY_ADAPTER(padapter);
1214         pmlmepriv = &padapter->mlmepriv;
1215
1216         if (time_before(pwrpriv->ips_deny_time, deny_time))
1217                 pwrpriv->ips_deny_time = deny_time;
1218
1219
1220         if (pwrpriv->ps_processing) {
1221                 DBG_871X("%s wait ps_processing...\n", __func__);
1222                 while (pwrpriv->ps_processing && jiffies_to_msecs(jiffies - start) <= 3000)
1223                         mdelay(10);
1224                 if (pwrpriv->ps_processing)
1225                         DBG_871X("%s wait ps_processing timeout\n", __func__);
1226                 else
1227                         DBG_871X("%s wait ps_processing done\n", __func__);
1228         }
1229
1230         if (!(pwrpriv->bInternalAutoSuspend) && pwrpriv->bInSuspend) {
1231                 DBG_871X("%s wait bInSuspend...\n", __func__);
1232                 while (pwrpriv->bInSuspend
1233                         && jiffies_to_msecs(jiffies - start) <= 3000
1234                 ) {
1235                         mdelay(10);
1236                 }
1237                 if (pwrpriv->bInSuspend)
1238                         DBG_871X("%s wait bInSuspend timeout\n", __func__);
1239                 else
1240                         DBG_871X("%s wait bInSuspend done\n", __func__);
1241         }
1242
1243         /* System suspend is not allowed to wakeup */
1244         if (!(pwrpriv->bInternalAutoSuspend) && pwrpriv->bInSuspend) {
1245                 ret = _FAIL;
1246                 goto exit;
1247         }
1248
1249         /* block??? */
1250         if (pwrpriv->bInternalAutoSuspend  && padapter->net_closed) {
1251                 ret = _FAIL;
1252                 goto exit;
1253         }
1254
1255         /* I think this should be check in IPS, LPS, autosuspend functions... */
1256         if (check_fwstate(pmlmepriv, _FW_LINKED)) {
1257                 ret = _SUCCESS;
1258                 goto exit;
1259         }
1260
1261         if (rf_off == pwrpriv->rf_pwrstate) {
1262                 {
1263                         DBG_8192C("%s call ips_leave....\n", __func__);
1264                         if (_FAIL ==  ips_leave(padapter)) {
1265                                 DBG_8192C("======> ips_leave fail.............\n");
1266                                 ret = _FAIL;
1267                                 goto exit;
1268                         }
1269                 }
1270         }
1271
1272         /* TODO: the following checking need to be merged... */
1273         if (padapter->bDriverStopped
1274                 || !padapter->bup
1275                 || !padapter->hw_init_completed
1276         ) {
1277                 DBG_8192C("%s: bDriverStopped =%d, bup =%d, hw_init_completed =%u\n"
1278                         , caller
1279                         , padapter->bDriverStopped
1280                         , padapter->bup
1281                         , padapter->hw_init_completed);
1282                 ret = false;
1283                 goto exit;
1284         }
1285
1286 exit:
1287         deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
1288         if (time_before(pwrpriv->ips_deny_time, deny_time))
1289                 pwrpriv->ips_deny_time = deny_time;
1290         return ret;
1291
1292 }
1293
1294 int rtw_pm_set_lps(struct adapter *padapter, u8 mode)
1295 {
1296         int     ret = 0;
1297         struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
1298
1299         if (mode < PS_MODE_NUM) {
1300                 if (pwrctrlpriv->power_mgnt != mode) {
1301                         if (PS_MODE_ACTIVE == mode)
1302                                 LeaveAllPowerSaveMode(padapter);
1303                         else
1304                                 pwrctrlpriv->LpsIdleCount = 2;
1305
1306                         pwrctrlpriv->power_mgnt = mode;
1307                         pwrctrlpriv->bLeisurePs =
1308                                 pwrctrlpriv->power_mgnt != PS_MODE_ACTIVE;
1309                 }
1310         } else
1311                 ret = -EINVAL;
1312
1313         return ret;
1314 }
1315
1316 int rtw_pm_set_ips(struct adapter *padapter, u8 mode)
1317 {
1318         struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
1319
1320         if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
1321                 rtw_ips_mode_req(pwrctrlpriv, mode);
1322                 DBG_871X("%s %s\n", __func__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
1323                 return 0;
1324         } else if (mode == IPS_NONE) {
1325                 rtw_ips_mode_req(pwrctrlpriv, mode);
1326                 DBG_871X("%s %s\n", __func__, "IPS_NONE");
1327                 if ((padapter->bSurpriseRemoved == 0) && (_FAIL == rtw_pwr_wakeup(padapter)))
1328                         return -EFAULT;
1329         } else
1330                 return -EINVAL;
1331
1332         return 0;
1333 }
1334
1335 /*
1336  * ATTENTION:
1337  *This function will request pwrctrl LOCK!
1338  */
1339 void rtw_ps_deny(struct adapter *padapter, enum PS_DENY_REASON reason)
1340 {
1341         struct pwrctrl_priv *pwrpriv;
1342
1343 /*      DBG_871X("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n", */
1344 /*              FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
1345
1346         pwrpriv = adapter_to_pwrctl(padapter);
1347
1348         mutex_lock(&pwrpriv->lock);
1349         if (pwrpriv->ps_deny & BIT(reason)) {
1350                 DBG_871X(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n",
1351                         FUNC_ADPT_ARG(padapter), reason);
1352         }
1353         pwrpriv->ps_deny |= BIT(reason);
1354         mutex_unlock(&pwrpriv->lock);
1355
1356 /*      DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */
1357 /*              FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
1358 }
1359
1360 /*
1361  * ATTENTION:
1362  *This function will request pwrctrl LOCK!
1363  */
1364 void rtw_ps_deny_cancel(struct adapter *padapter, enum PS_DENY_REASON reason)
1365 {
1366         struct pwrctrl_priv *pwrpriv;
1367
1368
1369 /*      DBG_871X("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n", */
1370 /*              FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
1371
1372         pwrpriv = adapter_to_pwrctl(padapter);
1373
1374         mutex_lock(&pwrpriv->lock);
1375         if ((pwrpriv->ps_deny & BIT(reason)) == 0) {
1376                 DBG_871X(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n",
1377                         FUNC_ADPT_ARG(padapter), reason);
1378         }
1379         pwrpriv->ps_deny &= ~BIT(reason);
1380         mutex_unlock(&pwrpriv->lock);
1381
1382 /*      DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */
1383 /*              FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
1384 }
1385
1386 /*
1387  * ATTENTION:
1388  *Before calling this function pwrctrl lock should be occupied already,
1389  *otherwise it may return incorrect value.
1390  */
1391 u32 rtw_ps_deny_get(struct adapter *padapter)
1392 {
1393         return adapter_to_pwrctl(padapter)->ps_deny;
1394 }