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 /* Interrupt safe routine to enter the leisure power save mode.*/ 411 static void rtl_lps_enter_core(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 448 /* Interrupt safe routine to leave the leisure power save mode.*/ 449 static void rtl_lps_leave_core(struct ieee80211_hw *hw) 450 { 451 struct rtl_priv *rtlpriv = rtl_priv(hw); 452 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 453 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); 454 unsigned long flag; 455 456 spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag); 457 458 if (ppsc->fwctrl_lps) { 459 if (ppsc->dot11_psmode != EACTIVE) { 460 461 /*FIX ME */ 462 /*rtlpriv->cfg->ops->enable_interrupt(hw); */ 463 464 if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM && 465 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) && 466 rtlhal->interface == INTF_PCI) { 467 rtlpriv->intf_ops->disable_aspm(hw); 468 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 469 } 470 471 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, 472 "Busy Traffic,Leave 802.11 power save..\n"); 473 474 rtl_lps_set_psmode(hw, EACTIVE); 475 } 476 } 477 spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag); 478 } 479 480 /* For sw LPS*/ 481 void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len) 482 { 483 struct rtl_priv *rtlpriv = rtl_priv(hw); 484 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 485 struct ieee80211_hdr *hdr = data; 486 struct ieee80211_tim_ie *tim_ie; 487 u8 *tim; 488 u8 tim_len; 489 bool u_buffed; 490 bool m_buffed; 491 492 if (mac->opmode != NL80211_IFTYPE_STATION) 493 return; 494 495 if (!rtlpriv->psc.swctrl_lps) 496 return; 497 498 if (rtlpriv->mac80211.link_state != MAC80211_LINKED) 499 return; 500 501 if (!rtlpriv->psc.sw_ps_enabled) 502 return; 503 504 if (rtlpriv->psc.fwctrl_lps) 505 return; 506 507 if (likely(!(hw->conf.flags & IEEE80211_CONF_PS))) 508 return; 509 510 /* check if this really is a beacon */ 511 if (!ieee80211_is_beacon(hdr->frame_control)) 512 return; 513 514 /* min. beacon length + FCS_LEN */ 515 if (len <= 40 + FCS_LEN) 516 return; 517 518 /* and only beacons from the associated BSSID, please */ 519 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid)) 520 return; 521 522 rtlpriv->psc.last_beacon = jiffies; 523 524 tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM); 525 if (!tim) 526 return; 527 528 if (tim[1] < sizeof(*tim_ie)) 529 return; 530 531 tim_len = tim[1]; 532 tim_ie = (struct ieee80211_tim_ie *) &tim[2]; 533 534 if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period)) 535 rtlpriv->psc.dtim_counter = tim_ie->dtim_count; 536 537 /* Check whenever the PHY can be turned off again. */ 538 539 /* 1. What about buffered unicast traffic for our AID? */ 540 u_buffed = ieee80211_check_tim(tim_ie, tim_len, 541 rtlpriv->mac80211.assoc_id); 542 543 /* 2. Maybe the AP wants to send multicast/broadcast data? */ 544 m_buffed = tim_ie->bitmap_ctrl & 0x01; 545 rtlpriv->psc.multi_buffered = m_buffed; 546 547 /* unicast will process by mac80211 through 548 * set ~IEEE80211_CONF_PS, So we just check 549 * multicast frames here */ 550 if (!m_buffed) { 551 /* back to low-power land. and delay is 552 * prevent null power save frame tx fail */ 553 queue_delayed_work(rtlpriv->works.rtl_wq, 554 &rtlpriv->works.ps_work, MSECS(5)); 555 } else { 556 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, 557 "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed); 558 } 559 } 560 EXPORT_SYMBOL_GPL(rtl_swlps_beacon); 561 562 void rtl_swlps_rf_awake(struct ieee80211_hw *hw) 563 { 564 struct rtl_priv *rtlpriv = rtl_priv(hw); 565 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 566 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 567 unsigned long flag; 568 569 if (!rtlpriv->psc.swctrl_lps) 570 return; 571 if (mac->link_state != MAC80211_LINKED) 572 return; 573 574 if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM && 575 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) { 576 rtlpriv->intf_ops->disable_aspm(hw); 577 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 578 } 579 580 spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag); 581 rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS); 582 spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag); 583 } 584 585 void rtl_swlps_rfon_wq_callback(void *data) 586 { 587 struct rtl_works *rtlworks = 588 container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq); 589 struct ieee80211_hw *hw = rtlworks->hw; 590 591 rtl_swlps_rf_awake(hw); 592 } 593 594 void rtl_swlps_rf_sleep(struct ieee80211_hw *hw) 595 { 596 struct rtl_priv *rtlpriv = rtl_priv(hw); 597 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 598 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw)); 599 unsigned long flag; 600 u8 sleep_intv; 601 602 if (!rtlpriv->psc.sw_ps_enabled) 603 return; 604 605 if ((rtlpriv->sec.being_setkey) || 606 (mac->opmode == NL80211_IFTYPE_ADHOC)) 607 return; 608 609 /*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */ 610 if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5)) 611 return; 612 613 if (rtlpriv->link_info.busytraffic) 614 return; 615 616 spin_lock(&rtlpriv->locks.rf_ps_lock); 617 if (rtlpriv->psc.rfchange_inprogress) { 618 spin_unlock(&rtlpriv->locks.rf_ps_lock); 619 return; 620 } 621 spin_unlock(&rtlpriv->locks.rf_ps_lock); 622 623 spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag); 624 rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS); 625 spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag); 626 627 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM && 628 !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) { 629 rtlpriv->intf_ops->enable_aspm(hw); 630 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM); 631 } 632 633 /* here is power save alg, when this beacon is DTIM 634 * we will set sleep time to dtim_period * n; 635 * when this beacon is not DTIM, we will set sleep 636 * time to sleep_intv = rtlpriv->psc.dtim_counter or 637 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */ 638 639 if (rtlpriv->psc.dtim_counter == 0) { 640 if (hw->conf.ps_dtim_period == 1) 641 sleep_intv = hw->conf.ps_dtim_period * 2; 642 else 643 sleep_intv = hw->conf.ps_dtim_period; 644 } else { 645 sleep_intv = rtlpriv->psc.dtim_counter; 646 } 647 648 if (sleep_intv > MAX_SW_LPS_SLEEP_INTV) 649 sleep_intv = MAX_SW_LPS_SLEEP_INTV; 650 651 /* this print should always be dtim_conter = 0 & 652 * sleep = dtim_period, that meaons, we should 653 * awake before every dtim */ 654 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, 655 "dtim_counter:%x will sleep :%d beacon_intv\n", 656 rtlpriv->psc.dtim_counter, sleep_intv); 657 658 /* we tested that 40ms is enough for sw & hw sw delay */ 659 queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq, 660 MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40)); 661 } 662 663 void rtl_lps_change_work_callback(struct work_struct *work) 664 { 665 struct rtl_works *rtlworks = 666 container_of(work, struct rtl_works, lps_change_work); 667 struct ieee80211_hw *hw = rtlworks->hw; 668 struct rtl_priv *rtlpriv = rtl_priv(hw); 669 670 if (rtlpriv->enter_ps) 671 rtl_lps_enter_core(hw); 672 else 673 rtl_lps_leave_core(hw); 674 } 675 EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback); 676 677 void rtl_lps_enter(struct ieee80211_hw *hw) 678 { 679 struct rtl_priv *rtlpriv = rtl_priv(hw); 680 681 if (!in_interrupt()) 682 return rtl_lps_enter_core(hw); 683 rtlpriv->enter_ps = true; 684 schedule_work(&rtlpriv->works.lps_change_work); 685 } 686 EXPORT_SYMBOL_GPL(rtl_lps_enter); 687 688 void rtl_lps_leave(struct ieee80211_hw *hw) 689 { 690 struct rtl_priv *rtlpriv = rtl_priv(hw); 691 692 if (!in_interrupt()) 693 return rtl_lps_leave_core(hw); 694 rtlpriv->enter_ps = false; 695 schedule_work(&rtlpriv->works.lps_change_work); 696 } 697 EXPORT_SYMBOL_GPL(rtl_lps_leave); 698 699 void rtl_swlps_wq_callback(void *data) 700 { 701 struct rtl_works *rtlworks = container_of_dwork_rtl(data, 702 struct rtl_works, 703 ps_work); 704 struct ieee80211_hw *hw = rtlworks->hw; 705 struct rtl_priv *rtlpriv = rtl_priv(hw); 706 bool ps = false; 707 708 ps = (hw->conf.flags & IEEE80211_CONF_PS); 709 710 /* we can sleep after ps null send ok */ 711 if (rtlpriv->psc.state_inap) { 712 rtl_swlps_rf_sleep(hw); 713 714 if (rtlpriv->psc.state && !ps) { 715 rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies - 716 rtlpriv->psc.last_action); 717 } 718 719 if (ps) 720 rtlpriv->psc.last_slept = jiffies; 721 722 rtlpriv->psc.last_action = jiffies; 723 rtlpriv->psc.state = ps; 724 } 725 } 726 727 static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data, 728 unsigned int len) 729 { 730 struct rtl_priv *rtlpriv = rtl_priv(hw); 731 struct ieee80211_mgmt *mgmt = data; 732 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 733 u8 *pos, *end, *ie; 734 u16 noa_len; 735 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09}; 736 u8 noa_num, index , i, noa_index = 0; 737 bool find_p2p_ie = false , find_p2p_ps_ie = false; 738 pos = (u8 *)mgmt->u.beacon.variable; 739 end = data + len; 740 ie = NULL; 741 742 while (pos + 1 < end) { 743 if (pos + 2 + pos[1] > end) 744 return; 745 746 if (pos[0] == 221 && pos[1] > 4) { 747 if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) { 748 ie = pos + 2+4; 749 break; 750 } 751 } 752 pos += 2 + pos[1]; 753 } 754 755 if (ie == NULL) 756 return; 757 find_p2p_ie = true; 758 /*to find noa ie*/ 759 while (ie + 1 < end) { 760 noa_len = READEF2BYTE((__le16 *)&ie[1]); 761 if (ie + 3 + ie[1] > end) 762 return; 763 764 if (ie[0] == 12) { 765 find_p2p_ps_ie = true; 766 if ((noa_len - 2) % 13 != 0) { 767 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, 768 "P2P notice of absence: invalid length.%d\n", 769 noa_len); 770 return; 771 } else { 772 noa_num = (noa_len - 2) / 13; 773 } 774 noa_index = ie[3]; 775 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == 776 P2P_PS_NONE || noa_index != p2pinfo->noa_index) { 777 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 778 "update NOA ie.\n"); 779 p2pinfo->noa_index = noa_index; 780 p2pinfo->opp_ps = (ie[4] >> 7); 781 p2pinfo->ctwindow = ie[4] & 0x7F; 782 p2pinfo->noa_num = noa_num; 783 index = 5; 784 for (i = 0; i < noa_num; i++) { 785 p2pinfo->noa_count_type[i] = 786 READEF1BYTE(ie+index); 787 index += 1; 788 p2pinfo->noa_duration[i] = 789 READEF4BYTE((__le32 *)ie+index); 790 index += 4; 791 p2pinfo->noa_interval[i] = 792 READEF4BYTE((__le32 *)ie+index); 793 index += 4; 794 p2pinfo->noa_start_time[i] = 795 READEF4BYTE((__le32 *)ie+index); 796 index += 4; 797 } 798 799 if (p2pinfo->opp_ps == 1) { 800 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW; 801 /* Driver should wait LPS entering 802 * CTWindow 803 */ 804 if (rtlpriv->psc.fw_current_inpsmode) 805 rtl_p2p_ps_cmd(hw, 806 P2P_PS_ENABLE); 807 } else if (p2pinfo->noa_num > 0) { 808 p2pinfo->p2p_ps_mode = P2P_PS_NOA; 809 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE); 810 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 811 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 812 } 813 } 814 break; 815 } 816 ie += 3 + noa_len; 817 } 818 819 if (find_p2p_ie == true) { 820 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) && 821 (find_p2p_ps_ie == false)) 822 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 823 } 824 } 825 826 static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data, 827 unsigned int len) 828 { 829 struct rtl_priv *rtlpriv = rtl_priv(hw); 830 struct ieee80211_mgmt *mgmt = data; 831 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 832 u8 noa_num, index , i , noa_index = 0; 833 u8 *pos, *end, *ie; 834 u16 noa_len; 835 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09}; 836 837 pos = (u8 *)&mgmt->u.action.category; 838 end = data + len; 839 ie = NULL; 840 841 if (pos[0] == 0x7f) { 842 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0) 843 ie = pos + 3+4; 844 } 845 846 if (ie == NULL) 847 return; 848 849 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n"); 850 /*to find noa ie*/ 851 while (ie + 1 < end) { 852 noa_len = READEF2BYTE((__le16 *)&ie[1]); 853 if (ie + 3 + ie[1] > end) 854 return; 855 856 if (ie[0] == 12) { 857 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n"); 858 RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ", 859 ie, noa_len); 860 if ((noa_len - 2) % 13 != 0) { 861 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 862 "P2P notice of absence: invalid length.%d\n", 863 noa_len); 864 return; 865 } else { 866 noa_num = (noa_len - 2) / 13; 867 } 868 noa_index = ie[3]; 869 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == 870 P2P_PS_NONE || noa_index != p2pinfo->noa_index) { 871 p2pinfo->noa_index = noa_index; 872 p2pinfo->opp_ps = (ie[4] >> 7); 873 p2pinfo->ctwindow = ie[4] & 0x7F; 874 p2pinfo->noa_num = noa_num; 875 index = 5; 876 for (i = 0; i < noa_num; i++) { 877 p2pinfo->noa_count_type[i] = 878 READEF1BYTE(ie+index); 879 index += 1; 880 p2pinfo->noa_duration[i] = 881 READEF4BYTE((__le32 *)ie+index); 882 index += 4; 883 p2pinfo->noa_interval[i] = 884 READEF4BYTE((__le32 *)ie+index); 885 index += 4; 886 p2pinfo->noa_start_time[i] = 887 READEF4BYTE((__le32 *)ie+index); 888 index += 4; 889 } 890 891 if (p2pinfo->opp_ps == 1) { 892 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW; 893 /* Driver should wait LPS entering 894 * CTWindow 895 */ 896 if (rtlpriv->psc.fw_current_inpsmode) 897 rtl_p2p_ps_cmd(hw, 898 P2P_PS_ENABLE); 899 } else if (p2pinfo->noa_num > 0) { 900 p2pinfo->p2p_ps_mode = P2P_PS_NOA; 901 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE); 902 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 903 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE); 904 } 905 } 906 break; 907 } 908 ie += 3 + noa_len; 909 } 910 } 911 912 void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state) 913 { 914 struct rtl_priv *rtlpriv = rtl_priv(hw); 915 struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw)); 916 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info); 917 918 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n" , p2p_ps_state); 919 switch (p2p_ps_state) { 920 case P2P_PS_DISABLE: 921 p2pinfo->p2p_ps_state = p2p_ps_state; 922 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 923 &p2p_ps_state); 924 p2pinfo->noa_index = 0; 925 p2pinfo->ctwindow = 0; 926 p2pinfo->opp_ps = 0; 927 p2pinfo->noa_num = 0; 928 p2pinfo->p2p_ps_mode = P2P_PS_NONE; 929 if (rtlps->fw_current_inpsmode) { 930 if (rtlps->smart_ps == 0) { 931 rtlps->smart_ps = 2; 932 rtlpriv->cfg->ops->set_hw_reg(hw, 933 HW_VAR_H2C_FW_PWRMODE, 934 &rtlps->pwr_mode); 935 } 936 937 } 938 break; 939 case P2P_PS_ENABLE: 940 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 941 p2pinfo->p2p_ps_state = p2p_ps_state; 942 943 if (p2pinfo->ctwindow > 0) { 944 if (rtlps->smart_ps != 0) { 945 rtlps->smart_ps = 0; 946 rtlpriv->cfg->ops->set_hw_reg(hw, 947 HW_VAR_H2C_FW_PWRMODE, 948 &rtlps->pwr_mode); 949 } 950 } 951 rtlpriv->cfg->ops->set_hw_reg(hw, 952 HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 953 &p2p_ps_state); 954 955 } 956 break; 957 case P2P_PS_SCAN: 958 case P2P_PS_SCAN_DONE: 959 case P2P_PS_ALLSTASLEEP: 960 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) { 961 p2pinfo->p2p_ps_state = p2p_ps_state; 962 rtlpriv->cfg->ops->set_hw_reg(hw, 963 HW_VAR_H2C_FW_P2P_PS_OFFLOAD, 964 &p2p_ps_state); 965 } 966 break; 967 default: 968 break; 969 } 970 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 971 "ctwindow %x oppps %x\n", 972 p2pinfo->ctwindow , p2pinfo->opp_ps); 973 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, 974 "count %x duration %x index %x interval %x start time %x noa num %x\n", 975 p2pinfo->noa_count_type[0], 976 p2pinfo->noa_duration[0], 977 p2pinfo->noa_index, 978 p2pinfo->noa_interval[0], 979 p2pinfo->noa_start_time[0], 980 p2pinfo->noa_num); 981 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n"); 982 } 983 984 void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len) 985 { 986 struct rtl_priv *rtlpriv = rtl_priv(hw); 987 struct rtl_mac *mac = rtl_mac(rtl_priv(hw)); 988 struct ieee80211_hdr *hdr = data; 989 990 if (!mac->p2p) 991 return; 992 if (mac->link_state != MAC80211_LINKED) 993 return; 994 /* min. beacon length + FCS_LEN */ 995 if (len <= 40 + FCS_LEN) 996 return; 997 998 /* and only beacons from the associated BSSID, please */ 999 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid)) 1000 return; 1001 1002 /* check if this really is a beacon */ 1003 if (!(ieee80211_is_beacon(hdr->frame_control) || 1004 ieee80211_is_probe_resp(hdr->frame_control) || 1005 ieee80211_is_action(hdr->frame_control))) 1006 return; 1007 1008 if (ieee80211_is_action(hdr->frame_control)) 1009 rtl_p2p_action_ie(hw , data , len - FCS_LEN); 1010 else 1011 rtl_p2p_noa_ie(hw , data , len - FCS_LEN); 1012 } 1013 EXPORT_SYMBOL_GPL(rtl_p2p_info); 1014