drivers/net/wireless/rtlwifi/ps.c

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