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