984 lines · c
1// SPDX-License-Identifier: GPL-2.02/* 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 10bool 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}45EXPORT_SYMBOL(rtl_ps_enable_nic);46 47bool 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}63EXPORT_SYMBOL(rtl_ps_disable_nic);64 65static 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 change75 *the RF state at one time, and others76 *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 avoid92 *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 150static 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 182void rtl_ips_nic_off_wq_callback(struct work_struct *work)183{184 struct rtl_works *rtlworks = container_of(work, struct rtl_works,185 ips_nic_off_wq.work);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 Sleep220 *(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 248void 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 us253 * to disable nic quickly after scan before linking,254 * this will cause link failed, so we delay 100ms here255 */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 without261 * nic_on may something wrong, like adhoc TP262 */263void 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}290EXPORT_SYMBOL_GPL(rtl_ips_nic_on);291 292/*for FW LPS*/293 294/*295 *Determine if we can set Fw into PS mode296 *in current condition.Return TRUE if it297 *can enter PS mode.298 */299static 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.*/325void 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 mode347 * Set RPWM to Fw to turn RF off and send H2C fw_pwrmode348 * cmd to set Fw into PS mode.349 *2. Leave PS mode350 * Send H2C fw_pwrmode cmd to Fw to set Fw into Active351 * 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.*/392static 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), because420 * 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.*/431static 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*/462void 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 through529 * set ~IEEE80211_CONF_PS, So we just check530 * multicast frames here */531 if (!m_buffed) {532 /* back to low-power land. and delay is533 * 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}541EXPORT_SYMBOL_GPL(rtl_swlps_beacon);542 543void 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 565void rtl_swlps_rfon_wq_callback(struct work_struct *work)566{567 struct rtl_works *rtlworks = container_of(work, struct rtl_works,568 ps_rfon_wq.work);569 struct ieee80211_hw *hw = rtlworks->hw;570 571 rtl_swlps_rf_awake(hw);572}573 574void 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 DTIM613 * we will set sleep time to dtim_period * n;614 * when this beacon is not DTIM, we will set sleep615 * time to sleep_intv = rtlpriv->psc.dtim_counter or616 * 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 else622 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 should632 * 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 642void 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 else652 rtl_lps_leave_core(hw);653}654EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback);655 656void rtl_lps_enter(struct ieee80211_hw *hw, bool may_block)657{658 struct rtl_priv *rtlpriv = rtl_priv(hw);659 660 if (may_block)661 return rtl_lps_enter_core(hw);662 rtlpriv->enter_ps = true;663 schedule_work(&rtlpriv->works.lps_change_work);664}665EXPORT_SYMBOL_GPL(rtl_lps_enter);666 667void rtl_lps_leave(struct ieee80211_hw *hw, bool may_block)668{669 struct rtl_priv *rtlpriv = rtl_priv(hw);670 671 if (may_block)672 return rtl_lps_leave_core(hw);673 rtlpriv->enter_ps = false;674 schedule_work(&rtlpriv->works.lps_change_work);675}676EXPORT_SYMBOL_GPL(rtl_lps_leave);677 678void rtl_swlps_wq_callback(struct work_struct *work)679{680 struct rtl_works *rtlworks = container_of(work, struct rtl_works,681 ps_work.work);682 struct ieee80211_hw *hw = rtlworks->hw;683 struct rtl_priv *rtlpriv = rtl_priv(hw);684 685 /* we can sleep after ps null send ok */686 if (rtlpriv->psc.state_inap)687 rtl_swlps_rf_sleep(hw);688}689 690static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,691 unsigned int len)692{693 struct rtl_priv *rtlpriv = rtl_priv(hw);694 struct ieee80211_mgmt *mgmt = data;695 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);696 u8 *pos, *end, *ie;697 u16 noa_len;698 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};699 u8 noa_num, index , i, noa_index = 0;700 bool find_p2p_ie = false , find_p2p_ps_ie = false;701 702 pos = (u8 *)mgmt->u.beacon.variable;703 end = data + len;704 ie = NULL;705 706 while (pos + 1 < end) {707 if (pos + 2 + pos[1] > end)708 return;709 710 if (pos[0] == 221 && pos[1] > 4) {711 if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {712 ie = pos + 2+4;713 break;714 }715 }716 pos += 2 + pos[1];717 }718 719 if (ie == NULL)720 return;721 find_p2p_ie = true;722 /*to find noa ie*/723 while (ie + 1 < end) {724 noa_len = le16_to_cpu(*((__le16 *)&ie[1]));725 if (ie + 3 + ie[1] > end)726 return;727 728 if (ie[0] == 12) {729 find_p2p_ps_ie = true;730 if ((noa_len - 2) % 13 != 0) {731 rtl_dbg(rtlpriv, COMP_INIT, DBG_LOUD,732 "P2P notice of absence: invalid length.%d\n",733 noa_len);734 return;735 } else {736 noa_num = (noa_len - 2) / 13;737 if (noa_num > P2P_MAX_NOA_NUM)738 noa_num = P2P_MAX_NOA_NUM;739 740 }741 noa_index = ie[3];742 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==743 P2P_PS_NONE || noa_index != p2pinfo->noa_index) {744 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD,745 "update NOA ie.\n");746 p2pinfo->noa_index = noa_index;747 p2pinfo->opp_ps = (ie[4] >> 7);748 p2pinfo->ctwindow = ie[4] & 0x7F;749 p2pinfo->noa_num = noa_num;750 index = 5;751 for (i = 0; i < noa_num; i++) {752 p2pinfo->noa_count_type[i] =753 *(u8 *)(ie + index);754 index += 1;755 p2pinfo->noa_duration[i] =756 le32_to_cpu(*(__le32 *)(ie + index));757 index += 4;758 p2pinfo->noa_interval[i] =759 le32_to_cpu(*(__le32 *)(ie + index));760 index += 4;761 p2pinfo->noa_start_time[i] =762 le32_to_cpu(*(__le32 *)(ie + index));763 index += 4;764 }765 766 if (p2pinfo->opp_ps == 1) {767 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;768 /* Driver should wait LPS entering769 * CTWindow770 */771 if (rtlpriv->psc.fw_current_inpsmode)772 rtl_p2p_ps_cmd(hw,773 P2P_PS_ENABLE);774 } else if (p2pinfo->noa_num > 0) {775 p2pinfo->p2p_ps_mode = P2P_PS_NOA;776 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);777 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {778 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);779 }780 }781 break;782 }783 ie += 3 + noa_len;784 }785 786 if (find_p2p_ie) {787 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&788 (!find_p2p_ps_ie))789 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);790 }791}792 793static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,794 unsigned int len)795{796 struct rtl_priv *rtlpriv = rtl_priv(hw);797 struct ieee80211_mgmt *mgmt = data;798 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);799 u8 noa_num, index , i , noa_index = 0;800 u8 *pos, *end, *ie;801 u16 noa_len;802 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};803 804 pos = (u8 *)&mgmt->u.action.category;805 end = data + len;806 ie = NULL;807 808 if (pos[0] == 0x7f) {809 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)810 ie = pos + 3+4;811 }812 813 if (ie == NULL)814 return;815 816 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n");817 /*to find noa ie*/818 while (ie + 1 < end) {819 noa_len = le16_to_cpu(*(__le16 *)&ie[1]);820 if (ie + 3 + ie[1] > end)821 return;822 823 if (ie[0] == 12) {824 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n");825 RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",826 ie, noa_len);827 if ((noa_len - 2) % 13 != 0) {828 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD,829 "P2P notice of absence: invalid length.%d\n",830 noa_len);831 return;832 } else {833 noa_num = (noa_len - 2) / 13;834 if (noa_num > P2P_MAX_NOA_NUM)835 noa_num = P2P_MAX_NOA_NUM;836 837 }838 noa_index = ie[3];839 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==840 P2P_PS_NONE || noa_index != p2pinfo->noa_index) {841 p2pinfo->noa_index = noa_index;842 p2pinfo->opp_ps = (ie[4] >> 7);843 p2pinfo->ctwindow = ie[4] & 0x7F;844 p2pinfo->noa_num = noa_num;845 index = 5;846 for (i = 0; i < noa_num; i++) {847 p2pinfo->noa_count_type[i] =848 *(u8 *)(ie + index);849 index += 1;850 p2pinfo->noa_duration[i] =851 le32_to_cpu(*(__le32 *)(ie + index));852 index += 4;853 p2pinfo->noa_interval[i] =854 le32_to_cpu(*(__le32 *)(ie + index));855 index += 4;856 p2pinfo->noa_start_time[i] =857 le32_to_cpu(*(__le32 *)(ie + index));858 index += 4;859 }860 861 if (p2pinfo->opp_ps == 1) {862 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;863 /* Driver should wait LPS entering864 * CTWindow865 */866 if (rtlpriv->psc.fw_current_inpsmode)867 rtl_p2p_ps_cmd(hw,868 P2P_PS_ENABLE);869 } else if (p2pinfo->noa_num > 0) {870 p2pinfo->p2p_ps_mode = P2P_PS_NOA;871 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);872 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {873 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);874 }875 }876 break;877 }878 ie += 3 + noa_len;879 }880}881 882void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state)883{884 struct rtl_priv *rtlpriv = rtl_priv(hw);885 struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));886 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);887 888 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n", p2p_ps_state);889 switch (p2p_ps_state) {890 case P2P_PS_DISABLE:891 p2pinfo->p2p_ps_state = p2p_ps_state;892 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD,893 &p2p_ps_state);894 p2pinfo->noa_index = 0;895 p2pinfo->ctwindow = 0;896 p2pinfo->opp_ps = 0;897 p2pinfo->noa_num = 0;898 p2pinfo->p2p_ps_mode = P2P_PS_NONE;899 if (rtlps->fw_current_inpsmode) {900 if (rtlps->smart_ps == 0) {901 rtlps->smart_ps = 2;902 rtlpriv->cfg->ops->set_hw_reg(hw,903 HW_VAR_H2C_FW_PWRMODE,904 &rtlps->pwr_mode);905 }906 907 }908 break;909 case P2P_PS_ENABLE:910 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {911 p2pinfo->p2p_ps_state = p2p_ps_state;912 913 if (p2pinfo->ctwindow > 0) {914 if (rtlps->smart_ps != 0) {915 rtlps->smart_ps = 0;916 rtlpriv->cfg->ops->set_hw_reg(hw,917 HW_VAR_H2C_FW_PWRMODE,918 &rtlps->pwr_mode);919 }920 }921 rtlpriv->cfg->ops->set_hw_reg(hw,922 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,923 &p2p_ps_state);924 925 }926 break;927 case P2P_PS_SCAN:928 case P2P_PS_SCAN_DONE:929 case P2P_PS_ALLSTASLEEP:930 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {931 p2pinfo->p2p_ps_state = p2p_ps_state;932 rtlpriv->cfg->ops->set_hw_reg(hw,933 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,934 &p2p_ps_state);935 }936 break;937 default:938 break;939 }940 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD,941 "ctwindow %x oppps %x\n",942 p2pinfo->ctwindow, p2pinfo->opp_ps);943 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD,944 "count %x duration %x index %x interval %x start time %x noa num %x\n",945 p2pinfo->noa_count_type[0],946 p2pinfo->noa_duration[0],947 p2pinfo->noa_index,948 p2pinfo->noa_interval[0],949 p2pinfo->noa_start_time[0],950 p2pinfo->noa_num);951 rtl_dbg(rtlpriv, COMP_FW, DBG_LOUD, "end\n");952}953 954void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)955{956 struct rtl_priv *rtlpriv = rtl_priv(hw);957 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));958 struct ieee80211_hdr *hdr = data;959 960 if (!mac->p2p)961 return;962 if (mac->link_state != MAC80211_LINKED)963 return;964 /* min. beacon length + FCS_LEN */965 if (len <= 40 + FCS_LEN)966 return;967 968 /* and only beacons from the associated BSSID, please */969 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))970 return;971 972 /* check if this really is a beacon */973 if (!(ieee80211_is_beacon(hdr->frame_control) ||974 ieee80211_is_probe_resp(hdr->frame_control) ||975 ieee80211_is_action(hdr->frame_control)))976 return;977 978 if (ieee80211_is_action(hdr->frame_control))979 rtl_p2p_action_ie(hw , data , len - FCS_LEN);980 else981 rtl_p2p_noa_ie(hw , data , len - FCS_LEN);982}983EXPORT_SYMBOL_GPL(rtl_p2p_info);984