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 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 change 75 *the RF state at one time, and others 76 *should wait to be executed. 77 */ 78 while (true) { 79 spin_lock(&rtlpriv->locks.rf_ps_lock); 80 if (ppsc->rfchange_inprogress) { 81 spin_unlock(&rtlpriv->locks.rf_ps_lock); 82 83 rtl_dbg(rtlpriv, COMP_ERR, DBG_WARNING, 84 "RF Change in progress! Wait to set..state_toset(%d).\n", 85 state_toset); 86 87 /* Set RF after the previous action is done. */ 88 while (ppsc->rfchange_inprogress) { 89 rfwait_cnt++; 90 mdelay(1); 91 /*Wait too long, return false to avoid 92 *to be stuck here. 93 */ 94 if (rfwait_cnt > 100) 95 return false; 96 } 97 } else { 98 ppsc->rfchange_inprogress = true; 99 spin_unlock(&rtlpriv->locks.rf_ps_lock); 100 break; 101 } 102 } 103 104 switch (state_toset) { 105 case ERFON: 106 ppsc->rfoff_reason &= (~changesource); 107 108 if ((changesource == RF_CHANGE_BY_HW) && 109 (ppsc->hwradiooff)) { 110 ppsc->hwradiooff = false; 111 } 112 113 if (!ppsc->rfoff_reason) { 114 ppsc->rfoff_reason = 0; 115 actionallowed = true; 116 } 117 118 break; 119 120 case ERFOFF: 121 122 if ((changesource == RF_CHANGE_BY_HW) && !ppsc->hwradiooff) { 123 ppsc->hwradiooff = true; 124 } 125 126 ppsc->rfoff_reason |= changesource; 127 actionallowed = true; 128 break; 129 130 case ERFSLEEP: 131 ppsc->rfoff_reason |= changesource; 132 actionallowed = true; 133 break; 134 135 default: 136 pr_err("switch case %#x not processed\n", state_toset); 137 break; 138 } 139 140 if (actionallowed) 141 rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset); 142 143 spin_lock(&rtlpriv->locks.rf_ps_lock); 144 ppsc->rfchange_inprogress = false; 145 spin_unlock(&rtlpriv->locks.rf_ps_lock); 146 147 return actionallowed; 148} 149 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 Sleep 220 *(2) swrf_processing (indicates the IPS is still under going) 221 *(3) Connectted (only disconnected can trigger IPS) 222 *(4) IBSS (send Beacon) 223 *(5) AP mode (send Beacon) 224 *(6) monitor mode (rcv packet) 225 */ 226 227 if (rtstate == ERFON && 228 !ppsc->swrf_processing && 229 (mac->link_state == MAC80211_NOLINK) && 230 !mac->act_scanning) { 231 rtl_dbg(rtlpriv, COMP_RF, DBG_TRACE, 232 "IPSEnter(): Turn off RF\n"); 233 234 ppsc->inactive_pwrstate = ERFOFF; 235 ppsc->in_powersavemode = true; 236 237 /* call before RF off */ 238 if (rtlpriv->cfg->ops->get_btc_status()) 239 rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv, 240 ppsc->inactive_pwrstate); 241 242 /*rtl_pci_reset_trx_ring(hw); */ 243 _rtl_ps_inactive_ps(hw); 244 } 245 } 246} 247 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 us 253 * to disable nic quickly after scan before linking, 254 * this will cause link failed, so we delay 100ms here 255 */ 256 queue_delayed_work(rtlpriv->works.rtl_wq, 257 &rtlpriv->works.ips_nic_off_wq, MSECS(100)); 258} 259 260/* NOTICE: any opmode should exc nic_on, or disable without 261 * nic_on may something wrong, like adhoc TP 262 */ 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 mode 296 *in current condition.Return TRUE if it 297 *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 mode 347 * Set RPWM to Fw to turn RF off and send H2C fw_pwrmode 348 * cmd to set Fw into PS mode. 349 *2. Leave PS mode 350 * Send H2C fw_pwrmode cmd to Fw to set Fw into Active 351 * mode and set RPWM to turn RF on. 352 */ 353 354 if ((ppsc->fwctrl_lps) && ppsc->report_linked) { 355 if (ppsc->dot11_psmode == EACTIVE) { 356 rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG, 357 "FW LPS leave ps_mode:%x\n", 358 FW_PS_ACTIVE_MODE); 359 enter_fwlps = false; 360 ppsc->pwr_mode = FW_PS_ACTIVE_MODE; 361 ppsc->smart_ps = 0; 362 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION, 363 (u8 *)(&enter_fwlps)); 364 if (ppsc->p2p_ps_info.opp_ps) 365 rtl_p2p_ps_cmd(hw , P2P_PS_ENABLE); 366 367 if (rtlpriv->cfg->ops->get_btc_status()) 368 rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode); 369 } else { 370 if (rtl_get_fwlps_doze(hw)) { 371 rtl_dbg(rtlpriv, COMP_RF, DBG_DMESG, 372 "FW LPS enter ps_mode:%x\n", 373 ppsc->fwctrl_psmode); 374 if (rtlpriv->cfg->ops->get_btc_status()) 375 rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode); 376 enter_fwlps = true; 377 ppsc->pwr_mode = ppsc->fwctrl_psmode; 378 ppsc->smart_ps = 2; 379 rtlpriv->cfg->ops->set_hw_reg(hw, 380 HW_VAR_FW_LPS_ACTION, 381 (u8 *)(&enter_fwlps)); 382 383 } else { 384 /* Reset the power save related parameters. */ 385 ppsc->dot11_psmode = EACTIVE; 386 } 387 } 388 } 389} 390 391/* Interrupt safe routine to enter the leisure power save mode.*/ 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), because 420 * bt_ccoexist may ask to enter lps. 421 * In normal case, this constraint move to rtl_lps_set_psmode(). 422 */ 423 rtl_dbg(rtlpriv, COMP_POWER, DBG_LOUD, 424 "Enter 802.11 power save mode...\n"); 425 rtl_lps_set_psmode(hw, EAUTOPS); 426 427 mutex_unlock(&rtlpriv->locks.lps_mutex); 428} 429 430/* Interrupt safe routine to leave the leisure power save mode.*/ 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 through 529 * set ~IEEE80211_CONF_PS, So we just check 530 * multicast frames here */ 531 if (!m_buffed) { 532 /* back to low-power land. and delay is 533 * prevent null power save frame tx fail */ 534 queue_delayed_work(rtlpriv->works.rtl_wq, 535 &rtlpriv->works.ps_work, MSECS(5)); 536 } else { 537 rtl_dbg(rtlpriv, COMP_POWER, DBG_DMESG, 538 "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed); 539 } 540} 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 DTIM 613 * we will set sleep time to dtim_period * n; 614 * when this beacon is not DTIM, we will set sleep 615 * time to sleep_intv = rtlpriv->psc.dtim_counter or 616 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */ 617 618 if (rtlpriv->psc.dtim_counter == 0) { 619 if (hw->conf.ps_dtim_period == 1) 620 sleep_intv = hw->conf.ps_dtim_period * 2; 621 else 622 sleep_intv = hw->conf.ps_dtim_period; 623 } else { 624 sleep_intv = rtlpriv->psc.dtim_counter; 625 } 626 627 if (sleep_intv > MAX_SW_LPS_SLEEP_INTV) 628 sleep_intv = MAX_SW_LPS_SLEEP_INTV; 629 630 /* this print should always be dtim_conter = 0 & 631 * sleep = dtim_period, that meaons, we should 632 * awake before every dtim */ 633 rtl_dbg(rtlpriv, COMP_POWER, DBG_DMESG, 634 "dtim_counter:%x will sleep :%d beacon_intv\n", 635 rtlpriv->psc.dtim_counter, sleep_intv); 636 637 /* we tested that 40ms is enough for sw & hw sw delay */ 638 queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq, 639 MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40)); 640} 641 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 else 652 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 entering 769 * CTWindow 770 */ 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 entering 864 * CTWindow 865 */ 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 else 981 rtl_p2p_noa_ie(hw , data , len - FCS_LEN); 982} 983EXPORT_SYMBOL_GPL(rtl_p2p_info); 984