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