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 38 /*<1> reset trx ring */ 39 if (rtlhal->interface == INTF_PCI) 40 rtlpriv->intf_ops->reset_trx_ring(hw); 41 42 if (is_hal_stop(rtlhal)) 43 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, 44 "Driver is already down!\n"); 45 46 /*<2> Enable Adapter */ 47 if (rtlpriv->cfg->ops->hw_init(hw)) 48 return false; 49 RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC); 50 51 /*<3> Enable Interrupt */ 52 rtlpriv->cfg->ops->enable_interrupt(hw); 53 54 /*<enable timer> */ 55 rtl_watch_dog_timer_callback((unsigned long)hw); 56 57 return true; 58 } 59 EXPORT_SYMBOL(rtl_ps_enable_nic); 60 61 bool rtl_ps_disable_nic(struct ieee80211_hw *hw) 62 { 63 struct rtl_priv *rtlpriv = rtl_priv(hw); 64 65 /*<1> Stop all timer */ 66 rtl_deinit_deferred_work(hw); 67 68 /*<2> Disable Interrupt */ 69 rtlpriv->cfg->ops->disable_interrupt(hw); 70 tasklet_kill(&rtlpriv->works.irq_tasklet); 71 72 /*<3> Disable Adapter */ 73 rtlpriv->cfg->ops->hw_disable(hw); 74 75 return true; 76 } 77 EXPORT_SYMBOL(rtl_ps_disable_nic); 78 79 static bool rtl_ps_set_rf_state(struct ieee80211_hw *hw, 80 enum rf_pwrstate state_toset, 81 u32 changesource) 82 { 83 struct rtl_priv *rtlpriv = rtl_priv(hw); 84 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 85 enum rf_pwrstate rtstate; 86 bool actionallowed = false; 87 u16 rfwait_cnt = 0; 88 89 /*Only one thread can change 90 *the RF state at one time, and others 91 *should wait to be executed. 92 */ 93 while (true) { 94 spin_lock(&rtlpriv->locks.rf_ps_lock); 95 if (ppsc->rfchange_inprogress) { 96 spin_unlock(&rtlpriv->locks.rf_ps_lock); 97 98 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, 99 "RF Change in progress! Wait to set..state_toset(%d).\n", 100 state_toset); 101 102 /* Set RF after the previous action is done. */ 103 while (ppsc->rfchange_inprogress) { 104 rfwait_cnt++; 105 mdelay(1); 106 /*Wait too long, return false to avoid 107 *to be stuck here. 108 */ 109 if (rfwait_cnt > 100) 110 return false; 111 } 112 } else { 113 ppsc->rfchange_inprogress = true; 114 spin_unlock(&rtlpriv->locks.rf_ps_lock); 115 break; 116 } 117 } 118 119 rtstate = ppsc->rfpwr_state; 120 121 switch (state_toset) { 122 case ERFON: 123 ppsc->rfoff_reason &= (~changesource); 124 125 if ((changesource == RF_CHANGE_BY_HW) && 126 (ppsc->hwradiooff)) { 127 ppsc->hwradiooff = false; 128 } 129 130 if (!ppsc->rfoff_reason) { 131 ppsc->rfoff_reason = 0; 132 actionallowed = true; 133 } 134 135 break; 136 137 case ERFOFF: 138 139 if ((changesource == RF_CHANGE_BY_HW) && !ppsc->hwradiooff) { 140 ppsc->hwradiooff = true; 141 } 142 143 ppsc->rfoff_reason |= changesource; 144 actionallowed = true; 145 break; 146 147 case ERFSLEEP: 148 ppsc->rfoff_reason |= changesource; 149 actionallowed = true; 150 break; 151 152 default: 153 RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, 154 "switch case %#x not processed\n", state_toset); 155 break; 156 } 157 158 if (actionallowed) 159 rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset); 160 161 spin_lock(&rtlpriv->locks.rf_ps_lock); 162 ppsc->rfchange_inprogress = false; 163 spin_unlock(&rtlpriv->locks.rf_ps_lock); 164 165 return actionallowed; 166 } 167 168 static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw) 169 { 170 struct rtl_priv *rtlpriv = rtl_priv(hw); 171 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); 172 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 173 174 ppsc->swrf_processing = true; 175 176 if (ppsc->inactive_pwrstate == ERFON && 177 rtlhal->interface == INTF_PCI) { 178 if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) && 179 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) && 180 rtlhal->interface == INTF_PCI) { 181 rtlpriv->intf_ops->disable_aspm(hw); 182 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 183 } 184 } 185 186 rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, 187 RF_CHANGE_BY_IPS); 188 189 if (ppsc->inactive_pwrstate == ERFOFF && 190 rtlhal->interface == INTF_PCI) { 191 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM && 192 !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) { 193 rtlpriv->intf_ops->enable_aspm(hw); 194 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 195 } 196 } 197 198 ppsc->swrf_processing = false; 199 } 200 201 void rtl_ips_nic_off_wq_callback(void *data) 202 { 203 struct rtl_works *rtlworks = 204 container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq); 205 struct ieee80211_hw *hw = rtlworks->hw; 206 struct rtl_priv *rtlpriv = rtl_priv(hw); 207 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); 208 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 209 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 210 enum rf_pwrstate rtstate; 211 212 if (mac->opmode != NL80211_IFTYPE_STATION) { 213 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, 214 "not station return\n"); 215 return; 216 } 217 218 if (mac->p2p_in_use) 219 return; 220 221 if (mac->link_state > MAC80211_NOLINK) 222 return; 223 224 if (is_hal_stop(rtlhal)) 225 return; 226 227 if (rtlpriv->sec.being_setkey) 228 return; 229 230 if (rtlpriv->cfg->ops->bt_coex_off_before_lps) 231 rtlpriv->cfg->ops->bt_coex_off_before_lps(hw); 232 233 if (ppsc->inactiveps) { 234 rtstate = ppsc->rfpwr_state; 235 236 /* 237 *Do not enter IPS in the following conditions: 238 *(1) RF is already OFF or Sleep 239 *(2) swrf_processing (indicates the IPS is still under going) 240 *(3) Connectted (only disconnected can trigger IPS) 241 *(4) IBSS (send Beacon) 242 *(5) AP mode (send Beacon) 243 *(6) monitor mode (rcv packet) 244 */ 245 246 if (rtstate == ERFON && 247 !ppsc->swrf_processing && 248 (mac->link_state == MAC80211_NOLINK) && 249 !mac->act_scanning) { 250 RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, 251 "IPSEnter(): Turn off RF\n"); 252 253 ppsc->inactive_pwrstate = ERFOFF; 254 ppsc->in_powersavemode = true; 255 256 /* call before RF off */ 257 if (rtlpriv->cfg->ops->get_btc_status()) 258 rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv, 259 ppsc->inactive_pwrstate); 260 261 /*rtl_pci_reset_trx_ring(hw); */ 262 _rtl_ps_inactive_ps(hw); 263 } 264 } 265 } 266 267 void rtl_ips_nic_off(struct ieee80211_hw *hw) 268 { 269 struct rtl_priv *rtlpriv = rtl_priv(hw); 270 271 /* because when link with ap, mac80211 will ask us 272 * to disable nic quickly after scan before linking, 273 * this will cause link failed, so we delay 100ms here 274 */ 275 queue_delayed_work(rtlpriv->works.rtl_wq, 276 &rtlpriv->works.ips_nic_off_wq, MSECS(100)); 277 } 278 279 /* NOTICE: any opmode should exc nic_on, or disable without 280 * nic_on may something wrong, like adhoc TP 281 */ 282 void rtl_ips_nic_on(struct ieee80211_hw *hw) 283 { 284 struct rtl_priv *rtlpriv = rtl_priv(hw); 285 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 286 enum rf_pwrstate rtstate; 287 288 cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq); 289 290 spin_lock(&rtlpriv->locks.ips_lock); 291 if (ppsc->inactiveps) { 292 rtstate = ppsc->rfpwr_state; 293 294 if (rtstate != ERFON && 295 !ppsc->swrf_processing && 296 ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) { 297 298 ppsc->inactive_pwrstate = ERFON; 299 ppsc->in_powersavemode = false; 300 _rtl_ps_inactive_ps(hw); 301 /* call after RF on */ 302 if (rtlpriv->cfg->ops->get_btc_status()) 303 rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv, 304 ppsc->inactive_pwrstate); 305 } 306 } 307 spin_unlock(&rtlpriv->locks.ips_lock); 308 } 309 EXPORT_SYMBOL_GPL(rtl_ips_nic_on); 310 311 /*for FW LPS*/ 312 313 /* 314 *Determine if we can set Fw into PS mode 315 *in current condition.Return TRUE if it 316 *can enter PS mode. 317 */ 318 static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw) 319 { 320 struct rtl_priv *rtlpriv = rtl_priv(hw); 321 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 322 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 323 u32 ps_timediff; 324 325 ps_timediff = jiffies_to_msecs(jiffies - 326 ppsc->last_delaylps_stamp_jiffies); 327 328 if (ps_timediff < 2000) { 329 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, 330 "Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n"); 331 return false; 332 } 333 334 if (mac->link_state != MAC80211_LINKED) 335 return false; 336 337 if (mac->opmode == NL80211_IFTYPE_ADHOC) 338 return false; 339 340 return true; 341 } 342 343 /* Change current and default preamble mode.*/ 344 void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode) 345 { 346 struct rtl_priv *rtlpriv = rtl_priv(hw); 347 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 348 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 349 bool enter_fwlps; 350 351 if (mac->opmode == NL80211_IFTYPE_ADHOC) 352 return; 353 354 if (mac->link_state != MAC80211_LINKED) 355 return; 356 357 if (ppsc->dot11_psmode == rt_psmode) 358 return; 359 360 /* Update power save mode configured. */ 361 ppsc->dot11_psmode = rt_psmode; 362 363 /* 364 *<FW control LPS> 365 *1. Enter PS mode 366 * Set RPWM to Fw to turn RF off and send H2C fw_pwrmode 367 * cmd to set Fw into PS mode. 368 *2. Leave PS mode 369 * Send H2C fw_pwrmode cmd to Fw to set Fw into Active 370 * mode and set RPWM to turn RF on. 371 */ 372 373 if ((ppsc->fwctrl_lps) && ppsc->report_linked) { 374 if (ppsc->dot11_psmode == EACTIVE) { 375 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG, 376 "FW LPS leave ps_mode:%x\n", 377 FW_PS_ACTIVE_MODE); 378 enter_fwlps = false; 379 ppsc->pwr_mode = FW_PS_ACTIVE_MODE; 380 ppsc->smart_ps = 0; 381 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION, 382 (u8 *)(&enter_fwlps)); 383 if (ppsc->p2p_ps_info.opp_ps) 384 rtl_p2p_ps_cmd(hw , P2P_PS_ENABLE); 385 386 if (rtlpriv->cfg->ops->get_btc_status()) 387 rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode); 388 } else { 389 if (rtl_get_fwlps_doze(hw)) { 390 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG, 391 "FW LPS enter ps_mode:%x\n", 392 ppsc->fwctrl_psmode); 393 if (rtlpriv->cfg->ops->get_btc_status()) 394 rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode); 395 enter_fwlps = true; 396 ppsc->pwr_mode = ppsc->fwctrl_psmode; 397 ppsc->smart_ps = 2; 398 rtlpriv->cfg->ops->set_hw_reg(hw, 399 HW_VAR_FW_LPS_ACTION, 400 (u8 *)(&enter_fwlps)); 401 402 } else { 403 /* Reset the power save related parameters. */ 404 ppsc->dot11_psmode = EACTIVE; 405 } 406 } 407 } 408 } 409 410 /*Enter the leisure power save mode.*/ 411 void rtl_lps_enter(struct ieee80211_hw *hw) 412 { 413 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 414 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 415 struct rtl_priv *rtlpriv = rtl_priv(hw); 416 unsigned long flag; 417 418 if (!ppsc->fwctrl_lps) 419 return; 420 421 if (rtlpriv->sec.being_setkey) 422 return; 423 424 if (rtlpriv->link_info.busytraffic) 425 return; 426 427 /*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */ 428 if (mac->cnt_after_linked < 5) 429 return; 430 431 if (mac->opmode == NL80211_IFTYPE_ADHOC) 432 return; 433 434 if (mac->link_state != MAC80211_LINKED) 435 return; 436 437 spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag); 438 439 if (ppsc->dot11_psmode == EACTIVE) { 440 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, 441 "Enter 802.11 power save mode...\n"); 442 rtl_lps_set_psmode(hw, EAUTOPS); 443 } 444 445 spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag); 446 } 447 EXPORT_SYMBOL(rtl_lps_enter); 448 449 /*Leave the leisure power save mode.*/ 450 void rtl_lps_leave(struct ieee80211_hw *hw) 451 { 452 struct rtl_priv *rtlpriv = rtl_priv(hw); 453 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 454 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); 455 unsigned long flag; 456 457 spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag); 458 459 if (ppsc->fwctrl_lps) { 460 if (ppsc->dot11_psmode != EACTIVE) { 461 462 /*FIX ME */ 463 /*rtlpriv->cfg->ops->enable_interrupt(hw); */ 464 465 if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM && 466 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) && 467 rtlhal->interface == INTF_PCI) { 468 rtlpriv->intf_ops->disable_aspm(hw); 469 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 470 } 471 472 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, 473 "Busy Traffic,Leave 802.11 power save..\n"); 474 475 rtl_lps_set_psmode(hw, EACTIVE); 476 } 477 } 478 spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag); 479 } 480 EXPORT_SYMBOL(rtl_lps_leave); 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(hw); 674 else 675 rtl_lps_leave(hw); 676 } 677 EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback); 678 679 void rtl_swlps_wq_callback(void *data) 680 { 681 struct rtl_works *rtlworks = container_of_dwork_rtl(data, 682 struct rtl_works, 683 ps_work); 684 struct ieee80211_hw *hw = rtlworks->hw; 685 struct rtl_priv *rtlpriv = rtl_priv(hw); 686 bool ps = false; 687 688 ps = (hw->conf.flags & IEEE80211_CONF_PS); 689 690 /* we can sleep after ps null send ok */ 691 if (rtlpriv->psc.state_inap) { 692 rtl_swlps_rf_sleep(hw); 693 694 if (rtlpriv->psc.state && !ps) { 695 rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies - 696 rtlpriv->psc.last_action); 697 } 698 699 if (ps) 700 rtlpriv->psc.last_slept = jiffies; 701 702 rtlpriv->psc.last_action = jiffies; 703 rtlpriv->psc.state = ps; 704 } 705 } 706 707 static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data, 708 unsigned int len) 709 { 710 struct rtl_priv *rtlpriv = rtl_priv(hw); 711 struct ieee80211_mgmt *mgmt = data; 712 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 713 u8 *pos, *end, *ie; 714 u16 noa_len; 715 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09}; 716 u8 noa_num, index , i, noa_index = 0; 717 bool find_p2p_ie = false , find_p2p_ps_ie = false; 718 pos = (u8 *)mgmt->u.beacon.variable; 719 end = data + len; 720 ie = NULL; 721 722 while (pos + 1 < end) { 723 if (pos + 2 + pos[1] > end) 724 return; 725 726 if (pos[0] == 221 && pos[1] > 4) { 727 if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) { 728 ie = pos + 2+4; 729 break; 730 } 731 } 732 pos += 2 + pos[1]; 733 } 734 735 if (ie == NULL) 736 return; 737 find_p2p_ie = true; 738 /*to find noa ie*/ 739 while (ie + 1 < end) { 740 noa_len = READEF2BYTE((__le16 *)&ie[1]); 741 if (ie + 3 + ie[1] > end) 742 return; 743 744 if (ie[0] == 12) { 745 find_p2p_ps_ie = true; 746 if ((noa_len - 2) % 13 != 0) { 747 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, 748 "P2P notice of absence: invalid length.%d\n", 749 noa_len); 750 return; 751 } else { 752 noa_num = (noa_len - 2) / 13; 753 } 754 noa_index = ie[3]; 755 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == 756 P2P_PS_NONE || noa_index != p2pinfo->noa_index) { 757 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 758 "update NOA ie.\n"); 759 p2pinfo->noa_index = noa_index; 760 p2pinfo->opp_ps = (ie[4] >> 7); 761 p2pinfo->ctwindow = ie[4] & 0x7F; 762 p2pinfo->noa_num = noa_num; 763 index = 5; 764 for (i = 0; i < noa_num; i++) { 765 p2pinfo->noa_count_type[i] = 766 READEF1BYTE(ie+index); 767 index += 1; 768 p2pinfo->noa_duration[i] = 769 READEF4BYTE((__le32 *)ie+index); 770 index += 4; 771 p2pinfo->noa_interval[i] = 772 READEF4BYTE((__le32 *)ie+index); 773 index += 4; 774 p2pinfo->noa_start_time[i] = 775 READEF4BYTE((__le32 *)ie+index); 776 index += 4; 777 } 778 779 if (p2pinfo->opp_ps == 1) { 780 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW; 781 /* Driver should wait LPS entering 782 * CTWindow 783 */ 784 if (rtlpriv->psc.fw_current_inpsmode) 785 rtl_p2p_ps_cmd(hw, 786 P2P_PS_ENABLE); 787 } else if (p2pinfo->noa_num > 0) { 788 p2pinfo->p2p_ps_mode = P2P_PS_NOA; 789 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE); 790 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 791 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 792 } 793 } 794 break; 795 } 796 ie += 3 + noa_len; 797 } 798 799 if (find_p2p_ie == true) { 800 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) && 801 (find_p2p_ps_ie == false)) 802 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 803 } 804 } 805 806 static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data, 807 unsigned int len) 808 { 809 struct rtl_priv *rtlpriv = rtl_priv(hw); 810 struct ieee80211_mgmt *mgmt = data; 811 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 812 u8 noa_num, index , i , noa_index = 0; 813 u8 *pos, *end, *ie; 814 u16 noa_len; 815 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09}; 816 817 pos = (u8 *)&mgmt->u.action.category; 818 end = data + len; 819 ie = NULL; 820 821 if (pos[0] == 0x7f) { 822 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0) 823 ie = pos + 3+4; 824 } 825 826 if (ie == NULL) 827 return; 828 829 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n"); 830 /*to find noa ie*/ 831 while (ie + 1 < end) { 832 noa_len = READEF2BYTE((__le16 *)&ie[1]); 833 if (ie + 3 + ie[1] > end) 834 return; 835 836 if (ie[0] == 12) { 837 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n"); 838 RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ", 839 ie, noa_len); 840 if ((noa_len - 2) % 13 != 0) { 841 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 842 "P2P notice of absence: invalid length.%d\n", 843 noa_len); 844 return; 845 } else { 846 noa_num = (noa_len - 2) / 13; 847 } 848 noa_index = ie[3]; 849 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == 850 P2P_PS_NONE || noa_index != p2pinfo->noa_index) { 851 p2pinfo->noa_index = noa_index; 852 p2pinfo->opp_ps = (ie[4] >> 7); 853 p2pinfo->ctwindow = ie[4] & 0x7F; 854 p2pinfo->noa_num = noa_num; 855 index = 5; 856 for (i = 0; i < noa_num; i++) { 857 p2pinfo->noa_count_type[i] = 858 READEF1BYTE(ie+index); 859 index += 1; 860 p2pinfo->noa_duration[i] = 861 READEF4BYTE((__le32 *)ie+index); 862 index += 4; 863 p2pinfo->noa_interval[i] = 864 READEF4BYTE((__le32 *)ie+index); 865 index += 4; 866 p2pinfo->noa_start_time[i] = 867 READEF4BYTE((__le32 *)ie+index); 868 index += 4; 869 } 870 871 if (p2pinfo->opp_ps == 1) { 872 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW; 873 /* Driver should wait LPS entering 874 * CTWindow 875 */ 876 if (rtlpriv->psc.fw_current_inpsmode) 877 rtl_p2p_ps_cmd(hw, 878 P2P_PS_ENABLE); 879 } else if (p2pinfo->noa_num > 0) { 880 p2pinfo->p2p_ps_mode = P2P_PS_NOA; 881 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE); 882 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 883 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 884 } 885 } 886 break; 887 } 888 ie += 3 + noa_len; 889 } 890 } 891 892 void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state) 893 { 894 struct rtl_priv *rtlpriv = rtl_priv(hw); 895 struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw)); 896 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 897 898 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n" , p2p_ps_state); 899 switch (p2p_ps_state) { 900 case P2P_PS_DISABLE: 901 p2pinfo->p2p_ps_state = p2p_ps_state; 902 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 903 &p2p_ps_state); 904 p2pinfo->noa_index = 0; 905 p2pinfo->ctwindow = 0; 906 p2pinfo->opp_ps = 0; 907 p2pinfo->noa_num = 0; 908 p2pinfo->p2p_ps_mode = P2P_PS_NONE; 909 if (rtlps->fw_current_inpsmode) { 910 if (rtlps->smart_ps == 0) { 911 rtlps->smart_ps = 2; 912 rtlpriv->cfg->ops->set_hw_reg(hw, 913 HW_VAR_H2C_FW_PWRMODE, 914 &rtlps->pwr_mode); 915 } 916 917 } 918 break; 919 case P2P_PS_ENABLE: 920 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 921 p2pinfo->p2p_ps_state = p2p_ps_state; 922 923 if (p2pinfo->ctwindow > 0) { 924 if (rtlps->smart_ps != 0) { 925 rtlps->smart_ps = 0; 926 rtlpriv->cfg->ops->set_hw_reg(hw, 927 HW_VAR_H2C_FW_PWRMODE, 928 &rtlps->pwr_mode); 929 } 930 } 931 rtlpriv->cfg->ops->set_hw_reg(hw, 932 HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 933 &p2p_ps_state); 934 935 } 936 break; 937 case P2P_PS_SCAN: 938 case P2P_PS_SCAN_DONE: 939 case P2P_PS_ALLSTASLEEP: 940 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 941 p2pinfo->p2p_ps_state = p2p_ps_state; 942 rtlpriv->cfg->ops->set_hw_reg(hw, 943 HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 944 &p2p_ps_state); 945 } 946 break; 947 default: 948 break; 949 } 950 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 951 "ctwindow %x oppps %x\n", 952 p2pinfo->ctwindow , p2pinfo->opp_ps); 953 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 954 "count %x duration %x index %x interval %x start time %x noa num %x\n", 955 p2pinfo->noa_count_type[0], 956 p2pinfo->noa_duration[0], 957 p2pinfo->noa_index, 958 p2pinfo->noa_interval[0], 959 p2pinfo->noa_start_time[0], 960 p2pinfo->noa_num); 961 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n"); 962 } 963 964 void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len) 965 { 966 struct rtl_priv *rtlpriv = rtl_priv(hw); 967 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 968 struct ieee80211_hdr *hdr = data; 969 970 if (!mac->p2p) 971 return; 972 if (mac->link_state != MAC80211_LINKED) 973 return; 974 /* min. beacon length + FCS_LEN */ 975 if (len <= 40 + FCS_LEN) 976 return; 977 978 /* and only beacons from the associated BSSID, please */ 979 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid)) 980 return; 981 982 /* check if this really is a beacon */ 983 if (!(ieee80211_is_beacon(hdr->frame_control) || 984 ieee80211_is_probe_resp(hdr->frame_control) || 985 ieee80211_is_action(hdr->frame_control))) 986 return; 987 988 if (ieee80211_is_action(hdr->frame_control)) 989 rtl_p2p_action_ie(hw , data , len - FCS_LEN); 990 else 991 rtl_p2p_noa_ie(hw , data , len - FCS_LEN); 992 } 993 EXPORT_SYMBOL_GPL(rtl_p2p_info); 994