LXR linux/drivers/staging/rtlwifi/ps.c

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