drivers/staging/rtl8723bs/core/rtw_pwrctrl.c

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