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