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