drivers/net/wireless/rtlwifi/ps.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2009-2012  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 false;
  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) && !ppsc->hwradiooff) {
 109                         ppsc->hwradiooff = true;
 110                 }
 111
 112                 ppsc->rfoff_reason |= changesource;
 113                 actionallowed = true;
 114                 break;
 115
 116         case ERFSLEEP:
 117                 ppsc->rfoff_reason |= changesource;
 118                 actionallowed = true;
 119                 break;
 120
 121         default:
 122                 RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 123                          "switch case not processed\n");
 124                 break;
 125         }
 126
 127         if (actionallowed)
 128                 rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
 129
 130         return actionallowed;
 131 }
 132 EXPORT_SYMBOL(rtl_ps_set_rf_state);
 133
 134 static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
 135 {
 136         struct rtl_priv *rtlpriv = rtl_priv(hw);
 137         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 138         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 139
 140         ppsc->swrf_processing = true;
 141
 142         if (ppsc->inactive_pwrstate == ERFON &&
 143             rtlhal->interface == INTF_PCI) {
 144                 if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
 145                     RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
 146                     rtlhal->interface == INTF_PCI) {
 147                         rtlpriv->intf_ops->disable_aspm(hw);
 148                         RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 149                 }
 150         }
 151
 152         rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, RF_CHANGE_BY_IPS);
 153
 154         if (ppsc->inactive_pwrstate == ERFOFF &&
 155             rtlhal->interface == INTF_PCI) {
 156                 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
 157                         !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 158                         rtlpriv->intf_ops->enable_aspm(hw);
 159                         RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 160                 }
 161         }
 162
 163         ppsc->swrf_processing = false;
 164 }
 165
 166 void rtl_ips_nic_off_wq_callback(void *data)
 167 {
 168         struct rtl_works *rtlworks =
 169             container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
 170         struct ieee80211_hw *hw = rtlworks->hw;
 171         struct rtl_priv *rtlpriv = rtl_priv(hw);
 172         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 173         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 174         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 175         enum rf_pwrstate rtstate;
 176
 177         if (mac->opmode != NL80211_IFTYPE_STATION) {
 178                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 179                          "not station return\n");
 180                 return;
 181         }
 182
 183         if (mac->p2p_in_use)
 184                 return;
 185
 186         if (mac->link_state > MAC80211_NOLINK)
 187                 return;
 188
 189         if (is_hal_stop(rtlhal))
 190                 return;
 191
 192         if (rtlpriv->sec.being_setkey)
 193                 return;
 194
 195         if (rtlpriv->cfg->ops->bt_coex_off_before_lps)
 196                 rtlpriv->cfg->ops->bt_coex_off_before_lps(hw);
 197
 198         if (ppsc->inactiveps) {
 199                 rtstate = ppsc->rfpwr_state;
 200
 201                 /*
 202                  *Do not enter IPS in the following conditions:
 203                  *(1) RF is already OFF or Sleep
 204                  *(2) swrf_processing (indicates the IPS is still under going)
 205                  *(3) Connectted (only disconnected can trigger IPS)
 206                  *(4) IBSS (send Beacon)
 207                  *(5) AP mode (send Beacon)
 208                  *(6) monitor mode (rcv packet)
 209                  */
 210
 211                 if (rtstate == ERFON &&
 212                     !ppsc->swrf_processing &&
 213                     (mac->link_state == MAC80211_NOLINK) &&
 214                     !mac->act_scanning) {
 215                         RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 216                                  "IPSEnter(): Turn off RF\n");
 217
 218                         ppsc->inactive_pwrstate = ERFOFF;
 219                         ppsc->in_powersavemode = true;
 220
 221                         /*rtl_pci_reset_trx_ring(hw); */
 222                         _rtl_ps_inactive_ps(hw);
 223                 }
 224         }
 225 }
 226
 227 void rtl_ips_nic_off(struct ieee80211_hw *hw)
 228 {
 229         struct rtl_priv *rtlpriv = rtl_priv(hw);
 230
 231         /*
 232          *because when link with ap, mac80211 will ask us
 233          *to disable nic quickly after scan before linking,
 234          *this will cause link failed, so we delay 100ms here
 235          */
 236         queue_delayed_work(rtlpriv->works.rtl_wq,
 237                            &rtlpriv->works.ips_nic_off_wq, MSECS(100));
 238 }
 239
 240 /* NOTICE: any opmode should exc nic_on, or disable without
 241  * nic_on may something wrong, like adhoc TP
 242  */
 243 void rtl_ips_nic_on(struct ieee80211_hw *hw)
 244 {
 245         struct rtl_priv *rtlpriv = rtl_priv(hw);
 246         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 247         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 248         enum rf_pwrstate rtstate;
 249         unsigned long flags;
 250
 251         if (mac->opmode != NL80211_IFTYPE_STATION)
 252                 return;
 253
 254         spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);
 255
 256         if (ppsc->inactiveps) {
 257                 rtstate = ppsc->rfpwr_state;
 258
 259                 if (rtstate != ERFON &&
 260                     !ppsc->swrf_processing &&
 261                     ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
 262
 263                         ppsc->inactive_pwrstate = ERFON;
 264                         ppsc->in_powersavemode = false;
 265
 266                         _rtl_ps_inactive_ps(hw);
 267                 }
 268         }
 269
 270         spin_unlock_irqrestore(&rtlpriv->locks.ips_lock, flags);
 271 }
 272 EXPORT_SYMBOL_GPL(rtl_ips_nic_on);
 273
 274 /*for FW LPS*/
 275
 276 /*
 277  *Determine if we can set Fw into PS mode
 278  *in current condition.Return TRUE if it
 279  *can enter PS mode.
 280  */
 281 static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
 282 {
 283         struct rtl_priv *rtlpriv = rtl_priv(hw);
 284         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 285         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 286         u32 ps_timediff;
 287
 288         ps_timediff = jiffies_to_msecs(jiffies -
 289                                        ppsc->last_delaylps_stamp_jiffies);
 290
 291         if (ps_timediff < 2000) {
 292                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 293                          "Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n");
 294                 return false;
 295         }
 296
 297         if (mac->link_state != MAC80211_LINKED)
 298                 return false;
 299
 300         if (mac->opmode == NL80211_IFTYPE_ADHOC)
 301                 return false;
 302
 303         return true;
 304 }
 305
 306 /* Change current and default preamble mode.*/
 307 static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 308 {
 309         struct rtl_priv *rtlpriv = rtl_priv(hw);
 310         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 311         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 312         bool enter_fwlps;
 313
 314         if (mac->opmode == NL80211_IFTYPE_ADHOC)
 315                 return;
 316
 317         if (mac->link_state != MAC80211_LINKED)
 318                 return;
 319
 320         if (ppsc->dot11_psmode == rt_psmode)
 321                 return;
 322
 323         /* Update power save mode configured. */
 324         ppsc->dot11_psmode = rt_psmode;
 325
 326         /*
 327          *<FW control LPS>
 328          *1. Enter PS mode
 329          *   Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
 330          *   cmd to set Fw into PS mode.
 331          *2. Leave PS mode
 332          *   Send H2C fw_pwrmode cmd to Fw to set Fw into Active
 333          *   mode and set RPWM to turn RF on.
 334          */
 335
 336         if ((ppsc->fwctrl_lps) && ppsc->report_linked) {
 337                 if (ppsc->dot11_psmode == EACTIVE) {
 338                         RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 339                                  "FW LPS leave ps_mode:%x\n",
 340                                  FW_PS_ACTIVE_MODE);
 341                         enter_fwlps = false;
 342                         ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
 343                         ppsc->smart_ps = 0;
 344                         rtlpriv->cfg->ops->set_hw_reg(hw,
 345                                                 HW_VAR_FW_LPS_ACTION,
 346                                                 (u8 *)(&enter_fwlps));
 347                         if (ppsc->p2p_ps_info.opp_ps)
 348                                 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 349
 350                 } else {
 351                         if (rtl_get_fwlps_doze(hw)) {
 352                                 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 353                                          "FW LPS enter ps_mode:%x\n",
 354                                          ppsc->fwctrl_psmode);
 355                                 enter_fwlps = true;
 356                                 ppsc->pwr_mode = ppsc->fwctrl_psmode;
 357                                 ppsc->smart_ps = 2;
 358                                 rtlpriv->cfg->ops->set_hw_reg(hw,
 359                                                         HW_VAR_FW_LPS_ACTION,
 360                                                         (u8 *)(&enter_fwlps));
 361
 362                         } else {
 363                                 /* Reset the power save related parameters. */
 364                                 ppsc->dot11_psmode = EACTIVE;
 365                         }
 366                 }
 367         }
 368 }
 369
 370 /*Enter the leisure power save mode.*/
 371 void rtl_lps_enter(struct ieee80211_hw *hw)
 372 {
 373         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 374         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 375         struct rtl_priv *rtlpriv = rtl_priv(hw);
 376
 377         if (!ppsc->fwctrl_lps)
 378                 return;
 379
 380         if (rtlpriv->sec.being_setkey)
 381                 return;
 382
 383         if (rtlpriv->link_info.busytraffic)
 384                 return;
 385
 386         /*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
 387         if (mac->cnt_after_linked < 5)
 388                 return;
 389
 390         if (mac->opmode == NL80211_IFTYPE_ADHOC)
 391                 return;
 392
 393         if (mac->link_state != MAC80211_LINKED)
 394                 return;
 395
 396         mutex_lock(&rtlpriv->locks.ps_mutex);
 397
 398         /* Idle for a while if we connect to AP a while ago. */
 399         if (mac->cnt_after_linked >= 2) {
 400                 if (ppsc->dot11_psmode == EACTIVE) {
 401                         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 402                                  "Enter 802.11 power save mode...\n");
 403
 404                         rtl_lps_set_psmode(hw, EAUTOPS);
 405                 }
 406         }
 407
 408         mutex_unlock(&rtlpriv->locks.ps_mutex);
 409 }
 410
 411 /*Leave the leisure power save mode.*/
 412 void rtl_lps_leave(struct ieee80211_hw *hw)
 413 {
 414         struct rtl_priv *rtlpriv = rtl_priv(hw);
 415         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 416         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 417
 418         mutex_lock(&rtlpriv->locks.ps_mutex);
 419
 420         if (ppsc->fwctrl_lps) {
 421                 if (ppsc->dot11_psmode != EACTIVE) {
 422
 423                         /*FIX ME */
 424                         rtlpriv->cfg->ops->enable_interrupt(hw);
 425
 426                         if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
 427                             RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
 428                             rtlhal->interface == INTF_PCI) {
 429                                 rtlpriv->intf_ops->disable_aspm(hw);
 430                                 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 431                         }
 432
 433                         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 434                                  "Busy Traffic,Leave 802.11 power save..\n");
 435
 436                         rtl_lps_set_psmode(hw, EACTIVE);
 437                 }
 438         }
 439         mutex_unlock(&rtlpriv->locks.ps_mutex);
 440 }
 441
 442 /* For sw LPS*/
 443 void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 444 {
 445         struct rtl_priv *rtlpriv = rtl_priv(hw);
 446         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 447         struct ieee80211_hdr *hdr = data;
 448         struct ieee80211_tim_ie *tim_ie;
 449         u8 *tim;
 450         u8 tim_len;
 451         bool u_buffed;
 452         bool m_buffed;
 453
 454         if (mac->opmode != NL80211_IFTYPE_STATION)
 455                 return;
 456
 457         if (!rtlpriv->psc.swctrl_lps)
 458                 return;
 459
 460         if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
 461                 return;
 462
 463         if (!rtlpriv->psc.sw_ps_enabled)
 464                 return;
 465
 466         if (rtlpriv->psc.fwctrl_lps)
 467                 return;
 468
 469         if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
 470                 return;
 471
 472         /* check if this really is a beacon */
 473         if (!ieee80211_is_beacon(hdr->frame_control))
 474                 return;
 475
 476         /* min. beacon length + FCS_LEN */
 477         if (len <= 40 + FCS_LEN)
 478                 return;
 479
 480         /* and only beacons from the associated BSSID, please */
 481         if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
 482                 return;
 483
 484         rtlpriv->psc.last_beacon = jiffies;
 485
 486         tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
 487         if (!tim)
 488                 return;
 489
 490         if (tim[1] < sizeof(*tim_ie))
 491                 return;
 492
 493         tim_len = tim[1];
 494         tim_ie = (struct ieee80211_tim_ie *) &tim[2];
 495
 496         if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
 497                 rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
 498
 499         /* Check whenever the PHY can be turned off again. */
 500
 501         /* 1. What about buffered unicast traffic for our AID? */
 502         u_buffed = ieee80211_check_tim(tim_ie, tim_len,
 503                                        rtlpriv->mac80211.assoc_id);
 504
 505         /* 2. Maybe the AP wants to send multicast/broadcast data? */
 506         m_buffed = tim_ie->bitmap_ctrl & 0x01;
 507         rtlpriv->psc.multi_buffered = m_buffed;
 508
 509         /* unicast will process by mac80211 through
 510          * set ~IEEE80211_CONF_PS, So we just check
 511          * multicast frames here */
 512         if (!m_buffed) {
 513                 /* back to low-power land. and delay is
 514                  * prevent null power save frame tx fail */
 515                 queue_delayed_work(rtlpriv->works.rtl_wq,
 516                                 &rtlpriv->works.ps_work, MSECS(5));
 517         } else {
 518                 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 519                          "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
 520         }
 521 }
 522 EXPORT_SYMBOL_GPL(rtl_swlps_beacon);
 523
 524 void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
 525 {
 526         struct rtl_priv *rtlpriv = rtl_priv(hw);
 527         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 528         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 529
 530         if (!rtlpriv->psc.swctrl_lps)
 531                 return;
 532         if (mac->link_state != MAC80211_LINKED)
 533                 return;
 534
 535         if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
 536                 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 537                 rtlpriv->intf_ops->disable_aspm(hw);
 538                 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 539         }
 540
 541         mutex_lock(&rtlpriv->locks.ps_mutex);
 542         rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
 543         mutex_unlock(&rtlpriv->locks.ps_mutex);
 544 }
 545
 546 void rtl_swlps_rfon_wq_callback(void *data)
 547 {
 548         struct rtl_works *rtlworks =
 549             container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
 550         struct ieee80211_hw *hw = rtlworks->hw;
 551
 552         rtl_swlps_rf_awake(hw);
 553 }
 554
 555 void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 556 {
 557         struct rtl_priv *rtlpriv = rtl_priv(hw);
 558         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 559         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 560         u8 sleep_intv;
 561
 562         if (!rtlpriv->psc.sw_ps_enabled)
 563                 return;
 564
 565         if ((rtlpriv->sec.being_setkey) ||
 566             (mac->opmode == NL80211_IFTYPE_ADHOC))
 567                 return;
 568
 569         /*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
 570         if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
 571                 return;
 572
 573         if (rtlpriv->link_info.busytraffic)
 574                 return;
 575
 576         mutex_lock(&rtlpriv->locks.ps_mutex);
 577         rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
 578         mutex_unlock(&rtlpriv->locks.ps_mutex);
 579
 580         if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
 581                 !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 582                 rtlpriv->intf_ops->enable_aspm(hw);
 583                 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 584         }
 585
 586         /* here is power save alg, when this beacon is DTIM
 587          * we will set sleep time to dtim_period * n;
 588          * when this beacon is not DTIM, we will set sleep
 589          * time to sleep_intv = rtlpriv->psc.dtim_counter or
 590          * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
 591
 592         if (rtlpriv->psc.dtim_counter == 0) {
 593                 if (hw->conf.ps_dtim_period == 1)
 594                         sleep_intv = hw->conf.ps_dtim_period * 2;
 595                 else
 596                         sleep_intv = hw->conf.ps_dtim_period;
 597         } else {
 598                 sleep_intv = rtlpriv->psc.dtim_counter;
 599         }
 600
 601         if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
 602                 sleep_intv = MAX_SW_LPS_SLEEP_INTV;
 603
 604         /* this print should always be dtim_conter = 0 &
 605          * sleep  = dtim_period, that meaons, we should
 606          * awake before every dtim */
 607         RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 608                  "dtim_counter:%x will sleep :%d beacon_intv\n",
 609                  rtlpriv->psc.dtim_counter, sleep_intv);
 610
 611         /* we tested that 40ms is enough for sw & hw sw delay */
 612         queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
 613                         MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
 614 }
 615
 616 void rtl_lps_change_work_callback(struct work_struct *work)
 617 {
 618         struct rtl_works *rtlworks =
 619             container_of(work, struct rtl_works, lps_change_work);
 620         struct ieee80211_hw *hw = rtlworks->hw;
 621         struct rtl_priv *rtlpriv = rtl_priv(hw);
 622
 623         if (rtlpriv->enter_ps)
 624                 rtl_lps_enter(hw);
 625         else
 626                 rtl_lps_leave(hw);
 627 }
 628 EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback);
 629
 630 void rtl_swlps_wq_callback(void *data)
 631 {
 632         struct rtl_works *rtlworks = container_of_dwork_rtl(data,
 633                                      struct rtl_works,
 634                                      ps_work);
 635         struct ieee80211_hw *hw = rtlworks->hw;
 636         struct rtl_priv *rtlpriv = rtl_priv(hw);
 637         bool ps = false;
 638
 639         ps = (hw->conf.flags & IEEE80211_CONF_PS);
 640
 641         /* we can sleep after ps null send ok */
 642         if (rtlpriv->psc.state_inap) {
 643                 rtl_swlps_rf_sleep(hw);
 644
 645                 if (rtlpriv->psc.state && !ps) {
 646                         rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
 647                                         rtlpriv->psc.last_action);
 648                 }
 649
 650                 if (ps)
 651                         rtlpriv->psc.last_slept = jiffies;
 652
 653                 rtlpriv->psc.last_action = jiffies;
 654                 rtlpriv->psc.state = ps;
 655         }
 656 }
 657
 658 static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
 659                            unsigned int len)
 660 {
 661         struct rtl_priv *rtlpriv = rtl_priv(hw);
 662         struct ieee80211_mgmt *mgmt = (void *)data;
 663         struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 664         u8 *pos, *end, *ie;
 665         u16 noa_len;
 666         static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
 667         u8 noa_num, index, i, noa_index = 0;
 668         bool find_p2p_ie = false , find_p2p_ps_ie = false;
 669         pos = (u8 *)mgmt->u.beacon.variable;
 670         end = data + len;
 671         ie = NULL;
 672
 673         while (pos + 1 < end) {
 674                 if (pos + 2 + pos[1] > end)
 675                         return;
 676
 677                 if (pos[0] == 221 && pos[1] > 4) {
 678                         if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
 679                                 ie = pos + 2+4;
 680                                 break;
 681                         }
 682                 }
 683                 pos += 2 + pos[1];
 684         }
 685
 686         if (ie == NULL)
 687                 return;
 688         find_p2p_ie = true;
 689         /*to find noa ie*/
 690         while (ie + 1 < end) {
 691                 noa_len = READEF2BYTE((__le16 *)&ie[1]);
 692                 if (ie + 3 + ie[1] > end)
 693                         return;
 694
 695                 if (ie[0] == 12) {
 696                         find_p2p_ps_ie = true;
 697                         if ((noa_len - 2) % 13 != 0) {
 698                                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 699                                          "P2P notice of absence: invalid length.%d\n",
 700                                          noa_len);
 701                                 return;
 702                         } else {
 703                                 noa_num = (noa_len - 2) / 13;
 704                         }
 705                         noa_index = ie[3];
 706                         if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
 707                             P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
 708                                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 709                                          "update NOA ie.\n");
 710                                 p2pinfo->noa_index = noa_index;
 711                                 p2pinfo->opp_ps = (ie[4] >> 7);
 712                                 p2pinfo->ctwindow = ie[4] & 0x7F;
 713                                 p2pinfo->noa_num = noa_num;
 714                                 index = 5;
 715                                 for (i = 0; i < noa_num; i++) {
 716                                         p2pinfo->noa_count_type[i] =
 717                                                  READEF1BYTE(ie+index);
 718                                         index += 1;
 719                                         p2pinfo->noa_duration[i] =
 720                                                  READEF4BYTE((__le32 *)ie+index);
 721                                         index += 4;
 722                                         p2pinfo->noa_interval[i] =
 723                                                  READEF4BYTE((__le32 *)ie+index);
 724                                         index += 4;
 725                                         p2pinfo->noa_start_time[i] =
 726                                                  READEF4BYTE((__le32 *)ie+index);
 727                                         index += 4;
 728                                 }
 729
 730                                 if (p2pinfo->opp_ps == 1) {
 731                                         p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
 732                                         /* Driver should wait LPS entering
 733                                          * CTWindow
 734                                          */
 735                                         if (rtlpriv->psc.fw_current_inpsmode)
 736                                                 rtl_p2p_ps_cmd(hw,
 737                                                                P2P_PS_ENABLE);
 738                                 } else if (p2pinfo->noa_num > 0) {
 739                                         p2pinfo->p2p_ps_mode = P2P_PS_NOA;
 740                                         rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 741                                 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 742                                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 743                                 }
 744                         }
 745                 break;
 746                 }
 747                 ie += 3 + noa_len;
 748         }
 749
 750         if (find_p2p_ie == true) {
 751                 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
 752                     (find_p2p_ps_ie == false))
 753                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 754         }
 755 }
 756
 757 static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
 758                               unsigned int len)
 759 {
 760         struct rtl_priv *rtlpriv = rtl_priv(hw);
 761         struct ieee80211_mgmt *mgmt = (void *)data;
 762         struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 763         u8 noa_num, index, i, noa_index = 0;
 764         u8 *pos, *end, *ie;
 765         u16 noa_len;
 766         static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
 767
 768         pos = (u8 *)&mgmt->u.action.category;
 769         end = data + len;
 770         ie = NULL;
 771
 772         if (pos[0] == 0x7f) {
 773                 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
 774                         ie = pos + 3+4;
 775         }
 776
 777         if (ie == NULL)
 778                 return;
 779
 780         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n");
 781         /*to find noa ie*/
 782         while (ie + 1 < end) {
 783                 noa_len = READEF2BYTE((__le16 *)&ie[1]);
 784                 if (ie + 3 + ie[1] > end)
 785                         return;
 786
 787                 if (ie[0] == 12) {
 788                         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n");
 789                         RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
 790                                       ie, noa_len);
 791                         if ((noa_len - 2) % 13 != 0) {
 792                                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 793                                          "P2P notice of absence: invalid length.%d\n",
 794                                          noa_len);
 795                                 return;
 796                         } else {
 797                                 noa_num = (noa_len - 2) / 13;
 798                         }
 799                         noa_index = ie[3];
 800                         if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
 801                             P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
 802                                 p2pinfo->noa_index = noa_index;
 803                                 p2pinfo->opp_ps = (ie[4] >> 7);
 804                                 p2pinfo->ctwindow = ie[4] & 0x7F;
 805                                 p2pinfo->noa_num = noa_num;
 806                                 index = 5;
 807                                 for (i = 0; i < noa_num; i++) {
 808                                         p2pinfo->noa_count_type[i] =
 809                                                          READEF1BYTE(ie+index);
 810                                         index += 1;
 811                                         p2pinfo->noa_duration[i] =
 812                                                          READEF4BYTE((__le32 *)ie+index);
 813                                         index += 4;
 814                                         p2pinfo->noa_interval[i] =
 815                                                          READEF4BYTE((__le32 *)ie+index);
 816                                         index += 4;
 817                                         p2pinfo->noa_start_time[i] =
 818                                                          READEF4BYTE((__le32 *)ie+index);
 819                                         index += 4;
 820                                 }
 821
 822                                 if (p2pinfo->opp_ps == 1) {
 823                                         p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
 824                                         /* Driver should wait LPS entering
 825                                          * CTWindow
 826                                          */
 827                                         if (rtlpriv->psc.fw_current_inpsmode)
 828                                                 rtl_p2p_ps_cmd(hw,
 829                                                                P2P_PS_ENABLE);
 830                                 } else if (p2pinfo->noa_num > 0) {
 831                                         p2pinfo->p2p_ps_mode = P2P_PS_NOA;
 832                                         rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 833                                 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 834                                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 835                                 }
 836                         }
 837                 break;
 838                 }
 839                 ie += 3 + noa_len;
 840         }
 841 }
 842
 843 void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 844 {
 845         struct rtl_priv *rtlpriv = rtl_priv(hw);
 846         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 847         struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 848
 849         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n", p2p_ps_state);
 850         switch (p2p_ps_state) {
 851         case P2P_PS_DISABLE:
 852                 p2pinfo->p2p_ps_state = p2p_ps_state;
 853                 rtlpriv->cfg->ops->set_hw_reg(hw,
 854                                  HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 855                                  (u8 *)(&p2p_ps_state));
 856
 857                 p2pinfo->noa_index = 0;
 858                 p2pinfo->ctwindow = 0;
 859                 p2pinfo->opp_ps = 0;
 860                 p2pinfo->noa_num = 0;
 861                 p2pinfo->p2p_ps_mode = P2P_PS_NONE;
 862                 if (rtlps->fw_current_inpsmode == true) {
 863                         if (rtlps->smart_ps == 0) {
 864                                 rtlps->smart_ps = 2;
 865                                 rtlpriv->cfg->ops->set_hw_reg(hw,
 866                                          HW_VAR_H2C_FW_PWRMODE,
 867                                          (u8 *)(&rtlps->pwr_mode));
 868                         }
 869                 }
 870                 break;
 871         case P2P_PS_ENABLE:
 872                 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 873                         p2pinfo->p2p_ps_state = p2p_ps_state;
 874
 875                         if (p2pinfo->ctwindow > 0) {
 876                                 if (rtlps->smart_ps != 0) {
 877                                         rtlps->smart_ps = 0;
 878                                         rtlpriv->cfg->ops->set_hw_reg(hw,
 879                                                  HW_VAR_H2C_FW_PWRMODE,
 880                                                  (u8 *)(&rtlps->pwr_mode));
 881                                 }
 882                         }
 883                         rtlpriv->cfg->ops->set_hw_reg(hw,
 884                                  HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 885                                  (u8 *)(&p2p_ps_state));
 886                 }
 887                 break;
 888         case P2P_PS_SCAN:
 889         case P2P_PS_SCAN_DONE:
 890         case P2P_PS_ALLSTASLEEP:
 891                 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 892                         p2pinfo->p2p_ps_state = p2p_ps_state;
 893                         rtlpriv->cfg->ops->set_hw_reg(hw,
 894                                  HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 895                                  (u8 *)(&p2p_ps_state));
 896                 }
 897                 break;
 898         default:
 899                 break;
 900         }
 901         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 902                  "ctwindow %x oppps %x\n", p2pinfo->ctwindow, p2pinfo->opp_ps);
 903         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 904                  "count %x duration %x index %x interval %x start time %x noa num %x\n",
 905                  p2pinfo->noa_count_type[0], p2pinfo->noa_duration[0],
 906                  p2pinfo->noa_index, p2pinfo->noa_interval[0],
 907                  p2pinfo->noa_start_time[0], p2pinfo->noa_num);
 908         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n");
 909 }
 910
 911 void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
 912 {
 913         struct rtl_priv *rtlpriv = rtl_priv(hw);
 914         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 915         struct ieee80211_hdr *hdr = (void *)data;
 916
 917         if (!mac->p2p)
 918                 return;
 919         if (mac->link_state != MAC80211_LINKED)
 920                 return;
 921         /* min. beacon length + FCS_LEN */
 922         if (len <= 40 + FCS_LEN)
 923                 return;
 924
 925         /* and only beacons from the associated BSSID, please */
 926         if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
 927                 return;
 928
 929         /* check if this really is a beacon */
 930         if (!(ieee80211_is_beacon(hdr->frame_control) ||
 931               ieee80211_is_probe_resp(hdr->frame_control) ||
 932               ieee80211_is_action(hdr->frame_control)))
 933                 return;
 934
 935         if (ieee80211_is_action(hdr->frame_control))
 936                 rtl_p2p_action_ie(hw, data, len - FCS_LEN);
 937         else
 938                 rtl_p2p_noa_ie(hw, data, len - FCS_LEN);
 939 }
 940 EXPORT_SYMBOL_GPL(rtl_p2p_info);