drivers/net/wireless/realtek/rtlwifi/ps.c

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