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