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, true);
  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_sync(&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 && rt_psmode == EACTIVE)
 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         /* Don't need to check (ppsc->dot11_psmode == EACTIVE), because
 442          * bt_ccoexist may ask to enter lps.
 443          * In normal case, this constraint move to rtl_lps_set_psmode().
 444          */
 445         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 446                  "Enter 802.11 power save mode...\n");
 447         rtl_lps_set_psmode(hw, EAUTOPS);
 448
 449         spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 450 }
 451
 452 /* Interrupt safe routine to leave the leisure power save mode.*/
 453 static void rtl_lps_leave_core(struct ieee80211_hw *hw)
 454 {
 455         struct rtl_priv *rtlpriv = rtl_priv(hw);
 456         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 457         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 458         unsigned long flag;
 459
 460         spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 461
 462         if (ppsc->fwctrl_lps) {
 463                 if (ppsc->dot11_psmode != EACTIVE) {
 464
 465                         /*FIX ME */
 466                         /*rtlpriv->cfg->ops->enable_interrupt(hw); */
 467
 468                         if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
 469                             RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
 470                             rtlhal->interface == INTF_PCI) {
 471                                 rtlpriv->intf_ops->disable_aspm(hw);
 472                                 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 473                         }
 474
 475                         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 476                                  "Busy Traffic,Leave 802.11 power save..\n");
 477
 478                         rtl_lps_set_psmode(hw, EACTIVE);
 479                 }
 480         }
 481         spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 482 }
 483
 484 /* For sw LPS*/
 485 void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 486 {
 487         struct rtl_priv *rtlpriv = rtl_priv(hw);
 488         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 489         struct ieee80211_hdr *hdr = data;
 490         struct ieee80211_tim_ie *tim_ie;
 491         u8 *tim;
 492         u8 tim_len;
 493         bool u_buffed;
 494         bool m_buffed;
 495
 496         if (mac->opmode != NL80211_IFTYPE_STATION)
 497                 return;
 498
 499         if (!rtlpriv->psc.swctrl_lps)
 500                 return;
 501
 502         if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
 503                 return;
 504
 505         if (!rtlpriv->psc.sw_ps_enabled)
 506                 return;
 507
 508         if (rtlpriv->psc.fwctrl_lps)
 509                 return;
 510
 511         if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
 512                 return;
 513
 514         /* check if this really is a beacon */
 515         if (!ieee80211_is_beacon(hdr->frame_control))
 516                 return;
 517
 518         /* min. beacon length + FCS_LEN */
 519         if (len <= 40 + FCS_LEN)
 520                 return;
 521
 522         /* and only beacons from the associated BSSID, please */
 523         if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
 524                 return;
 525
 526         rtlpriv->psc.last_beacon = jiffies;
 527
 528         tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
 529         if (!tim)
 530                 return;
 531
 532         if (tim[1] < sizeof(*tim_ie))
 533                 return;
 534
 535         tim_len = tim[1];
 536         tim_ie = (struct ieee80211_tim_ie *) &tim[2];
 537
 538         if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
 539                 rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
 540
 541         /* Check whenever the PHY can be turned off again. */
 542
 543         /* 1. What about buffered unicast traffic for our AID? */
 544         u_buffed = ieee80211_check_tim(tim_ie, tim_len,
 545                                        rtlpriv->mac80211.assoc_id);
 546
 547         /* 2. Maybe the AP wants to send multicast/broadcast data? */
 548         m_buffed = tim_ie->bitmap_ctrl & 0x01;
 549         rtlpriv->psc.multi_buffered = m_buffed;
 550
 551         /* unicast will process by mac80211 through
 552          * set ~IEEE80211_CONF_PS, So we just check
 553          * multicast frames here */
 554         if (!m_buffed) {
 555                 /* back to low-power land. and delay is
 556                  * prevent null power save frame tx fail */
 557                 queue_delayed_work(rtlpriv->works.rtl_wq,
 558                                    &rtlpriv->works.ps_work, MSECS(5));
 559         } else {
 560                 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 561                          "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
 562         }
 563 }
 564 EXPORT_SYMBOL_GPL(rtl_swlps_beacon);
 565
 566 void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
 567 {
 568         struct rtl_priv *rtlpriv = rtl_priv(hw);
 569         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 570         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 571         unsigned long flag;
 572
 573         if (!rtlpriv->psc.swctrl_lps)
 574                 return;
 575         if (mac->link_state != MAC80211_LINKED)
 576                 return;
 577
 578         if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
 579             RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 580                 rtlpriv->intf_ops->disable_aspm(hw);
 581                 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 582         }
 583
 584         spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 585         rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
 586         spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 587 }
 588
 589 void rtl_swlps_rfon_wq_callback(void *data)
 590 {
 591         struct rtl_works *rtlworks =
 592             container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
 593         struct ieee80211_hw *hw = rtlworks->hw;
 594
 595         rtl_swlps_rf_awake(hw);
 596 }
 597
 598 void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 599 {
 600         struct rtl_priv *rtlpriv = rtl_priv(hw);
 601         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 602         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 603         unsigned long flag;
 604         u8 sleep_intv;
 605
 606         if (!rtlpriv->psc.sw_ps_enabled)
 607                 return;
 608
 609         if ((rtlpriv->sec.being_setkey) ||
 610             (mac->opmode == NL80211_IFTYPE_ADHOC))
 611                 return;
 612
 613         /*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
 614         if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
 615                 return;
 616
 617         if (rtlpriv->link_info.busytraffic)
 618                 return;
 619
 620         spin_lock(&rtlpriv->locks.rf_ps_lock);
 621         if (rtlpriv->psc.rfchange_inprogress) {
 622                 spin_unlock(&rtlpriv->locks.rf_ps_lock);
 623                 return;
 624         }
 625         spin_unlock(&rtlpriv->locks.rf_ps_lock);
 626
 627         spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 628         rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
 629         spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 630
 631         if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
 632             !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 633                 rtlpriv->intf_ops->enable_aspm(hw);
 634                 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 635         }
 636
 637         /* here is power save alg, when this beacon is DTIM
 638          * we will set sleep time to dtim_period * n;
 639          * when this beacon is not DTIM, we will set sleep
 640          * time to sleep_intv = rtlpriv->psc.dtim_counter or
 641          * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
 642
 643         if (rtlpriv->psc.dtim_counter == 0) {
 644                 if (hw->conf.ps_dtim_period == 1)
 645                         sleep_intv = hw->conf.ps_dtim_period * 2;
 646                 else
 647                         sleep_intv = hw->conf.ps_dtim_period;
 648         } else {
 649                 sleep_intv = rtlpriv->psc.dtim_counter;
 650         }
 651
 652         if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
 653                 sleep_intv = MAX_SW_LPS_SLEEP_INTV;
 654
 655         /* this print should always be dtim_conter = 0 &
 656          * sleep  = dtim_period, that meaons, we should
 657          * awake before every dtim */
 658         RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 659                  "dtim_counter:%x will sleep :%d beacon_intv\n",
 660                   rtlpriv->psc.dtim_counter, sleep_intv);
 661
 662         /* we tested that 40ms is enough for sw & hw sw delay */
 663         queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
 664                         MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
 665 }
 666
 667 void rtl_lps_change_work_callback(struct work_struct *work)
 668 {
 669         struct rtl_works *rtlworks =
 670             container_of(work, struct rtl_works, lps_change_work);
 671         struct ieee80211_hw *hw = rtlworks->hw;
 672         struct rtl_priv *rtlpriv = rtl_priv(hw);
 673
 674         if (rtlpriv->enter_ps)
 675                 rtl_lps_enter_core(hw);
 676         else
 677                 rtl_lps_leave_core(hw);
 678 }
 679 EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback);
 680
 681 void rtl_lps_enter(struct ieee80211_hw *hw)
 682 {
 683         struct rtl_priv *rtlpriv = rtl_priv(hw);
 684
 685         if (!in_interrupt())
 686                 return rtl_lps_enter_core(hw);
 687         rtlpriv->enter_ps = true;
 688         schedule_work(&rtlpriv->works.lps_change_work);
 689 }
 690 EXPORT_SYMBOL_GPL(rtl_lps_enter);
 691
 692 void rtl_lps_leave(struct ieee80211_hw *hw)
 693 {
 694         struct rtl_priv *rtlpriv = rtl_priv(hw);
 695
 696         if (!in_interrupt())
 697                 return rtl_lps_leave_core(hw);
 698         rtlpriv->enter_ps = false;
 699         schedule_work(&rtlpriv->works.lps_change_work);
 700 }
 701 EXPORT_SYMBOL_GPL(rtl_lps_leave);
 702
 703 void rtl_swlps_wq_callback(void *data)
 704 {
 705         struct rtl_works *rtlworks = container_of_dwork_rtl(data,
 706                                      struct rtl_works,
 707                                      ps_work);
 708         struct ieee80211_hw *hw = rtlworks->hw;
 709         struct rtl_priv *rtlpriv = rtl_priv(hw);
 710         bool ps = false;
 711
 712         ps = (hw->conf.flags & IEEE80211_CONF_PS);
 713
 714         /* we can sleep after ps null send ok */
 715         if (rtlpriv->psc.state_inap) {
 716                 rtl_swlps_rf_sleep(hw);
 717
 718                 if (rtlpriv->psc.state && !ps) {
 719                         rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
 720                                                  rtlpriv->psc.last_action);
 721                 }
 722
 723                 if (ps)
 724                         rtlpriv->psc.last_slept = jiffies;
 725
 726                 rtlpriv->psc.last_action = jiffies;
 727                 rtlpriv->psc.state = ps;
 728         }
 729 }
 730
 731 static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
 732                            unsigned int len)
 733 {
 734         struct rtl_priv *rtlpriv = rtl_priv(hw);
 735         struct ieee80211_mgmt *mgmt = data;
 736         struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 737         u8 *pos, *end, *ie;
 738         u16 noa_len;
 739         static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
 740         u8 noa_num, index , i, noa_index = 0;
 741         bool find_p2p_ie = false , find_p2p_ps_ie = false;
 742         pos = (u8 *)mgmt->u.beacon.variable;
 743         end = data + len;
 744         ie = NULL;
 745
 746         while (pos + 1 < end) {
 747                 if (pos + 2 + pos[1] > end)
 748                         return;
 749
 750                 if (pos[0] == 221 && pos[1] > 4) {
 751                         if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
 752                                 ie = pos + 2+4;
 753                                 break;
 754                         }
 755                 }
 756                 pos += 2 + pos[1];
 757         }
 758
 759         if (ie == NULL)
 760                 return;
 761         find_p2p_ie = true;
 762         /*to find noa ie*/
 763         while (ie + 1 < end) {
 764                 noa_len = READEF2BYTE((__le16 *)&ie[1]);
 765                 if (ie + 3 + ie[1] > end)
 766                         return;
 767
 768                 if (ie[0] == 12) {
 769                         find_p2p_ps_ie = true;
 770                         if ((noa_len - 2) % 13 != 0) {
 771                                 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 772                                          "P2P notice of absence: invalid length.%d\n",
 773                                          noa_len);
 774                                 return;
 775                         } else {
 776                                 noa_num = (noa_len - 2) / 13;
 777                                 if (noa_num > P2P_MAX_NOA_NUM)
 778                                         noa_num = P2P_MAX_NOA_NUM;
 779
 780                         }
 781                         noa_index = ie[3];
 782                         if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
 783                             P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
 784                                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 785                                          "update NOA ie.\n");
 786                                 p2pinfo->noa_index = noa_index;
 787                                 p2pinfo->opp_ps = (ie[4] >> 7);
 788                                 p2pinfo->ctwindow = ie[4] & 0x7F;
 789                                 p2pinfo->noa_num = noa_num;
 790                                 index = 5;
 791                                 for (i = 0; i < noa_num; i++) {
 792                                         p2pinfo->noa_count_type[i] =
 793                                                         READEF1BYTE(ie+index);
 794                                         index += 1;
 795                                         p2pinfo->noa_duration[i] =
 796                                                  READEF4BYTE((__le32 *)ie+index);
 797                                         index += 4;
 798                                         p2pinfo->noa_interval[i] =
 799                                                  READEF4BYTE((__le32 *)ie+index);
 800                                         index += 4;
 801                                         p2pinfo->noa_start_time[i] =
 802                                                  READEF4BYTE((__le32 *)ie+index);
 803                                         index += 4;
 804                                 }
 805
 806                                 if (p2pinfo->opp_ps == 1) {
 807                                         p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
 808                                         /* Driver should wait LPS entering
 809                                          * CTWindow
 810                                          */
 811                                         if (rtlpriv->psc.fw_current_inpsmode)
 812                                                 rtl_p2p_ps_cmd(hw,
 813                                                                P2P_PS_ENABLE);
 814                                 } else if (p2pinfo->noa_num > 0) {
 815                                         p2pinfo->p2p_ps_mode = P2P_PS_NOA;
 816                                         rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 817                                 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 818                                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 819                                 }
 820                         }
 821                         break;
 822                 }
 823                 ie += 3 + noa_len;
 824         }
 825
 826         if (find_p2p_ie == true) {
 827                 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
 828                     (find_p2p_ps_ie == false))
 829                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 830         }
 831 }
 832
 833 static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
 834                               unsigned int len)
 835 {
 836         struct rtl_priv *rtlpriv = rtl_priv(hw);
 837         struct ieee80211_mgmt *mgmt = data;
 838         struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 839         u8 noa_num, index , i , noa_index = 0;
 840         u8 *pos, *end, *ie;
 841         u16 noa_len;
 842         static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
 843
 844         pos = (u8 *)&mgmt->u.action.category;
 845         end = data + len;
 846         ie = NULL;
 847
 848         if (pos[0] == 0x7f) {
 849                 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
 850                         ie = pos + 3+4;
 851         }
 852
 853         if (ie == NULL)
 854                 return;
 855
 856         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n");
 857         /*to find noa ie*/
 858         while (ie + 1 < end) {
 859                 noa_len = READEF2BYTE((__le16 *)&ie[1]);
 860                 if (ie + 3 + ie[1] > end)
 861                         return;
 862
 863                 if (ie[0] == 12) {
 864                         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n");
 865                         RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
 866                                       ie, noa_len);
 867                         if ((noa_len - 2) % 13 != 0) {
 868                                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 869                                          "P2P notice of absence: invalid length.%d\n",
 870                                          noa_len);
 871                                 return;
 872                         } else {
 873                                 noa_num = (noa_len - 2) / 13;
 874                                 if (noa_num > P2P_MAX_NOA_NUM)
 875                                         noa_num = P2P_MAX_NOA_NUM;
 876
 877                         }
 878                         noa_index = ie[3];
 879                         if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
 880                             P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
 881                                 p2pinfo->noa_index = noa_index;
 882                                 p2pinfo->opp_ps = (ie[4] >> 7);
 883                                 p2pinfo->ctwindow = ie[4] & 0x7F;
 884                                 p2pinfo->noa_num = noa_num;
 885                                 index = 5;
 886                                 for (i = 0; i < noa_num; i++) {
 887                                         p2pinfo->noa_count_type[i] =
 888                                                         READEF1BYTE(ie+index);
 889                                         index += 1;
 890                                         p2pinfo->noa_duration[i] =
 891                                                          READEF4BYTE((__le32 *)ie+index);
 892                                         index += 4;
 893                                         p2pinfo->noa_interval[i] =
 894                                                          READEF4BYTE((__le32 *)ie+index);
 895                                         index += 4;
 896                                         p2pinfo->noa_start_time[i] =
 897                                                          READEF4BYTE((__le32 *)ie+index);
 898                                         index += 4;
 899                                 }
 900
 901                                 if (p2pinfo->opp_ps == 1) {
 902                                         p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
 903                                         /* Driver should wait LPS entering
 904                                          * CTWindow
 905                                          */
 906                                         if (rtlpriv->psc.fw_current_inpsmode)
 907                                                 rtl_p2p_ps_cmd(hw,
 908                                                                P2P_PS_ENABLE);
 909                                 } else if (p2pinfo->noa_num > 0) {
 910                                         p2pinfo->p2p_ps_mode = P2P_PS_NOA;
 911                                         rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 912                                 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 913                                         rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 914                                 }
 915                         }
 916                         break;
 917                 }
 918                 ie += 3 + noa_len;
 919         }
 920 }
 921
 922 void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state)
 923 {
 924         struct rtl_priv *rtlpriv = rtl_priv(hw);
 925         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 926         struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 927
 928         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n" , p2p_ps_state);
 929         switch (p2p_ps_state) {
 930         case P2P_PS_DISABLE:
 931                 p2pinfo->p2p_ps_state = p2p_ps_state;
 932                 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 933                                               &p2p_ps_state);
 934                 p2pinfo->noa_index = 0;
 935                 p2pinfo->ctwindow = 0;
 936                 p2pinfo->opp_ps = 0;
 937                 p2pinfo->noa_num = 0;
 938                 p2pinfo->p2p_ps_mode = P2P_PS_NONE;
 939                 if (rtlps->fw_current_inpsmode) {
 940                         if (rtlps->smart_ps == 0) {
 941                                 rtlps->smart_ps = 2;
 942                                 rtlpriv->cfg->ops->set_hw_reg(hw,
 943                                          HW_VAR_H2C_FW_PWRMODE,
 944                                          &rtlps->pwr_mode);
 945                         }
 946
 947                 }
 948                 break;
 949         case P2P_PS_ENABLE:
 950                 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 951                         p2pinfo->p2p_ps_state = p2p_ps_state;
 952
 953                         if (p2pinfo->ctwindow > 0) {
 954                                 if (rtlps->smart_ps != 0) {
 955                                         rtlps->smart_ps = 0;
 956                                         rtlpriv->cfg->ops->set_hw_reg(hw,
 957                                                  HW_VAR_H2C_FW_PWRMODE,
 958                                                  &rtlps->pwr_mode);
 959                                 }
 960                         }
 961                         rtlpriv->cfg->ops->set_hw_reg(hw,
 962                                  HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 963                                  &p2p_ps_state);
 964
 965                 }
 966                 break;
 967         case P2P_PS_SCAN:
 968         case P2P_PS_SCAN_DONE:
 969         case P2P_PS_ALLSTASLEEP:
 970                 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
 971                         p2pinfo->p2p_ps_state = p2p_ps_state;
 972                         rtlpriv->cfg->ops->set_hw_reg(hw,
 973                                  HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 974                                  &p2p_ps_state);
 975                 }
 976                 break;
 977         default:
 978                 break;
 979         }
 980         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 981                  "ctwindow %x oppps %x\n",
 982                  p2pinfo->ctwindow , p2pinfo->opp_ps);
 983         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 984                  "count %x duration %x index %x interval %x start time %x noa num %x\n",
 985                  p2pinfo->noa_count_type[0],
 986                  p2pinfo->noa_duration[0],
 987                  p2pinfo->noa_index,
 988                  p2pinfo->noa_interval[0],
 989                  p2pinfo->noa_start_time[0],
 990                  p2pinfo->noa_num);
 991         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n");
 992 }
 993
 994 void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
 995 {
 996         struct rtl_priv *rtlpriv = rtl_priv(hw);
 997         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 998         struct ieee80211_hdr *hdr = data;
 999
1000         if (!mac->p2p)
1001                 return;
1002         if (mac->link_state != MAC80211_LINKED)
1003                 return;
1004         /* min. beacon length + FCS_LEN */
1005         if (len <= 40 + FCS_LEN)
1006                 return;
1007
1008         /* and only beacons from the associated BSSID, please */
1009         if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
1010                 return;
1011
1012         /* check if this really is a beacon */
1013         if (!(ieee80211_is_beacon(hdr->frame_control) ||
1014               ieee80211_is_probe_resp(hdr->frame_control) ||
1015               ieee80211_is_action(hdr->frame_control)))
1016                 return;
1017
1018         if (ieee80211_is_action(hdr->frame_control))
1019                 rtl_p2p_action_ie(hw , data , len - FCS_LEN);
1020         else
1021                 rtl_p2p_noa_ie(hw , data , len - FCS_LEN);
1022 }
1023 EXPORT_SYMBOL_GPL(rtl_p2p_info);