drivers/net/wireless/rtlwifi/ps.c

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