1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30#include <linux/export.h>
31#include "wifi.h"
32#include "base.h"
33#include "ps.h"
34
35
36
37
38
39
40
41bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
42 u8 faversion, u8 interface_type,
43 struct wlan_pwr_cfg pwrcfgcmd[])
44{
45 struct wlan_pwr_cfg cfg_cmd = {0};
46 bool polling_bit = false;
47 u32 ary_idx = 0;
48 u8 value = 0;
49 u32 offset = 0;
50 u32 polling_count = 0;
51 u32 max_polling_cnt = 5000;
52
53 do {
54 cfg_cmd = pwrcfgcmd[ary_idx];
55 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
56 "rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), famsk(%#x),"
57 "interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
58 GET_PWR_CFG_OFFSET(cfg_cmd),
59 GET_PWR_CFG_CUT_MASK(cfg_cmd),
60 GET_PWR_CFG_FAB_MASK(cfg_cmd),
61 GET_PWR_CFG_INTF_MASK(cfg_cmd),
62 GET_PWR_CFG_BASE(cfg_cmd), GET_PWR_CFG_CMD(cfg_cmd),
63 GET_PWR_CFG_MASK(cfg_cmd), GET_PWR_CFG_VALUE(cfg_cmd));
64
65 if ((GET_PWR_CFG_FAB_MASK(cfg_cmd)&faversion) &&
66 (GET_PWR_CFG_CUT_MASK(cfg_cmd)&cut_version) &&
67 (GET_PWR_CFG_INTF_MASK(cfg_cmd)&interface_type)) {
68 switch (GET_PWR_CFG_CMD(cfg_cmd)) {
69 case PWR_CMD_READ:
70 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
71 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
72 break;
73 case PWR_CMD_WRITE:
74 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
75 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
76 offset = GET_PWR_CFG_OFFSET(cfg_cmd);
77
78
79 value = rtl_read_byte(rtlpriv, offset);
80 value &= (~(GET_PWR_CFG_MASK(cfg_cmd)));
81 value |= (GET_PWR_CFG_VALUE(cfg_cmd) &
82 GET_PWR_CFG_MASK(cfg_cmd));
83
84
85 rtl_write_byte(rtlpriv, offset, value);
86 break;
87 case PWR_CMD_POLLING:
88 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
89 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
90 polling_bit = false;
91 offset = GET_PWR_CFG_OFFSET(cfg_cmd);
92
93 do {
94 value = rtl_read_byte(rtlpriv, offset);
95
96 value &= GET_PWR_CFG_MASK(cfg_cmd);
97 if (value ==
98 (GET_PWR_CFG_VALUE(cfg_cmd)
99 & GET_PWR_CFG_MASK(cfg_cmd)))
100 polling_bit = true;
101 else
102 udelay(10);
103
104 if (polling_count++ > max_polling_cnt)
105 return false;
106 } while (!polling_bit);
107 break;
108 case PWR_CMD_DELAY:
109 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
110 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
111 if (GET_PWR_CFG_VALUE(cfg_cmd) ==
112 PWRSEQ_DELAY_US)
113 udelay(GET_PWR_CFG_OFFSET(cfg_cmd));
114 else
115 mdelay(GET_PWR_CFG_OFFSET(cfg_cmd));
116 break;
117 case PWR_CMD_END:
118 RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
119 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
120 return true;
121 default:
122 RT_ASSERT(false,
123 "rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
124 break;
125 }
126
127 }
128 ary_idx++;
129 } while (1);
130
131 return true;
132}
133EXPORT_SYMBOL(rtl_hal_pwrseqcmdparsing);
134
135bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
136{
137 struct rtl_priv *rtlpriv = rtl_priv(hw);
138 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
139 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
140
141
142 if (rtlhal->interface == INTF_PCI)
143 rtlpriv->intf_ops->reset_trx_ring(hw);
144
145 if (is_hal_stop(rtlhal))
146 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
147 "Driver is already down!\n");
148
149
150 if (rtlpriv->cfg->ops->hw_init(hw))
151 return false;
152 RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
153
154
155 rtlpriv->cfg->ops->enable_interrupt(hw);
156
157
158 rtl_watch_dog_timer_callback((unsigned long)hw);
159
160 return true;
161}
162EXPORT_SYMBOL(rtl_ps_enable_nic);
163
164bool rtl_ps_disable_nic(struct ieee80211_hw *hw)
165{
166 struct rtl_priv *rtlpriv = rtl_priv(hw);
167
168
169 rtl_deinit_deferred_work(hw);
170
171
172 rtlpriv->cfg->ops->disable_interrupt(hw);
173 tasklet_kill(&rtlpriv->works.irq_tasklet);
174
175
176 rtlpriv->cfg->ops->hw_disable(hw);
177
178 return true;
179}
180EXPORT_SYMBOL(rtl_ps_disable_nic);
181
182bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
183 enum rf_pwrstate state_toset,
184 u32 changesource)
185{
186 struct rtl_priv *rtlpriv = rtl_priv(hw);
187 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
188 bool actionallowed = false;
189
190 switch (state_toset) {
191 case ERFON:
192 ppsc->rfoff_reason &= (~changesource);
193
194 if ((changesource == RF_CHANGE_BY_HW) &&
195 (ppsc->hwradiooff)) {
196 ppsc->hwradiooff = false;
197 }
198
199 if (!ppsc->rfoff_reason) {
200 ppsc->rfoff_reason = 0;
201 actionallowed = true;
202 }
203
204 break;
205
206 case ERFOFF:
207
208 if ((changesource == RF_CHANGE_BY_HW) && !ppsc->hwradiooff) {
209 ppsc->hwradiooff = true;
210 }
211
212 ppsc->rfoff_reason |= changesource;
213 actionallowed = true;
214 break;
215
216 case ERFSLEEP:
217 ppsc->rfoff_reason |= changesource;
218 actionallowed = true;
219 break;
220
221 default:
222 RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
223 "switch case not processed\n");
224 break;
225 }
226
227 if (actionallowed)
228 rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
229
230 return actionallowed;
231}
232EXPORT_SYMBOL(rtl_ps_set_rf_state);
233
234static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
235{
236 struct rtl_priv *rtlpriv = rtl_priv(hw);
237 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
238 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
239
240 ppsc->swrf_processing = true;
241
242 if (ppsc->inactive_pwrstate == ERFON &&
243 rtlhal->interface == INTF_PCI) {
244 if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
245 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
246 rtlhal->interface == INTF_PCI) {
247 rtlpriv->intf_ops->disable_aspm(hw);
248 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
249 }
250 }
251
252 rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, RF_CHANGE_BY_IPS);
253
254 if (ppsc->inactive_pwrstate == ERFOFF &&
255 rtlhal->interface == INTF_PCI) {
256 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
257 !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
258 rtlpriv->intf_ops->enable_aspm(hw);
259 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
260 }
261 }
262
263 ppsc->swrf_processing = false;
264}
265
266void rtl_ips_nic_off_wq_callback(void *data)
267{
268 struct rtl_works *rtlworks =
269 container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
270 struct ieee80211_hw *hw = rtlworks->hw;
271 struct rtl_priv *rtlpriv = rtl_priv(hw);
272 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
273 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
274 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
275 enum rf_pwrstate rtstate;
276
277 if (mac->opmode != NL80211_IFTYPE_STATION) {
278 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
279 "not station return\n");
280 return;
281 }
282
283 if (mac->p2p_in_use)
284 return;
285
286 if (mac->link_state > MAC80211_NOLINK)
287 return;
288
289 if (is_hal_stop(rtlhal))
290 return;
291
292 if (rtlpriv->sec.being_setkey)
293 return;
294
295 if (rtlpriv->cfg->ops->bt_coex_off_before_lps)
296 rtlpriv->cfg->ops->bt_coex_off_before_lps(hw);
297
298 if (ppsc->inactiveps) {
299 rtstate = ppsc->rfpwr_state;
300
301
302
303
304
305
306
307
308
309
310
311 if (rtstate == ERFON &&
312 !ppsc->swrf_processing &&
313 (mac->link_state == MAC80211_NOLINK) &&
314 !mac->act_scanning) {
315 RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
316 "IPSEnter(): Turn off RF\n");
317
318 ppsc->inactive_pwrstate = ERFOFF;
319 ppsc->in_powersavemode = true;
320
321
322 _rtl_ps_inactive_ps(hw);
323 }
324 }
325}
326
327void rtl_ips_nic_off(struct ieee80211_hw *hw)
328{
329 struct rtl_priv *rtlpriv = rtl_priv(hw);
330
331
332
333
334
335
336 queue_delayed_work(rtlpriv->works.rtl_wq,
337 &rtlpriv->works.ips_nic_off_wq, MSECS(100));
338}
339
340
341
342
343void rtl_ips_nic_on(struct ieee80211_hw *hw)
344{
345 struct rtl_priv *rtlpriv = rtl_priv(hw);
346 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
347 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
348 enum rf_pwrstate rtstate;
349 unsigned long flags;
350
351 if (mac->opmode != NL80211_IFTYPE_STATION)
352 return;
353
354 spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);
355
356 if (ppsc->inactiveps) {
357 rtstate = ppsc->rfpwr_state;
358
359 if (rtstate != ERFON &&
360 !ppsc->swrf_processing &&
361 ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
362
363 ppsc->inactive_pwrstate = ERFON;
364 ppsc->in_powersavemode = false;
365
366 _rtl_ps_inactive_ps(hw);
367 }
368 }
369
370 spin_unlock_irqrestore(&rtlpriv->locks.ips_lock, flags);
371}
372EXPORT_SYMBOL_GPL(rtl_ips_nic_on);
373
374
375
376
377
378
379
380
381static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
382{
383 struct rtl_priv *rtlpriv = rtl_priv(hw);
384 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
385 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
386 u32 ps_timediff;
387
388 ps_timediff = jiffies_to_msecs(jiffies -
389 ppsc->last_delaylps_stamp_jiffies);
390
391 if (ps_timediff < 2000) {
392 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
393 "Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n");
394 return false;
395 }
396
397 if (mac->link_state != MAC80211_LINKED)
398 return false;
399
400 if (mac->opmode == NL80211_IFTYPE_ADHOC)
401 return false;
402
403 return true;
404}
405
406
407static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
408{
409 struct rtl_priv *rtlpriv = rtl_priv(hw);
410 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
411 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
412 bool enter_fwlps;
413
414 if (mac->opmode == NL80211_IFTYPE_ADHOC)
415 return;
416
417 if (mac->link_state != MAC80211_LINKED)
418 return;
419
420 if (ppsc->dot11_psmode == rt_psmode)
421 return;
422
423
424 ppsc->dot11_psmode = rt_psmode;
425
426
427
428
429
430
431
432
433
434
435
436 if ((ppsc->fwctrl_lps) && ppsc->report_linked) {
437 if (ppsc->dot11_psmode == EACTIVE) {
438 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
439 "FW LPS leave ps_mode:%x\n",
440 FW_PS_ACTIVE_MODE);
441 enter_fwlps = false;
442 ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
443 ppsc->smart_ps = 0;
444 rtlpriv->cfg->ops->set_hw_reg(hw,
445 HW_VAR_FW_LPS_ACTION,
446 (u8 *)(&enter_fwlps));
447 if (ppsc->p2p_ps_info.opp_ps)
448 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
449
450 } else {
451 if (rtl_get_fwlps_doze(hw)) {
452 RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
453 "FW LPS enter ps_mode:%x\n",
454 ppsc->fwctrl_psmode);
455 enter_fwlps = true;
456 ppsc->pwr_mode = ppsc->fwctrl_psmode;
457 ppsc->smart_ps = 2;
458 rtlpriv->cfg->ops->set_hw_reg(hw,
459 HW_VAR_FW_LPS_ACTION,
460 (u8 *)(&enter_fwlps));
461
462 } else {
463
464 ppsc->dot11_psmode = EACTIVE;
465 }
466 }
467 }
468}
469
470
471void rtl_lps_enter(struct ieee80211_hw *hw)
472{
473 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
474 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
475 struct rtl_priv *rtlpriv = rtl_priv(hw);
476
477 if (!ppsc->fwctrl_lps)
478 return;
479
480 if (rtlpriv->sec.being_setkey)
481 return;
482
483 if (rtlpriv->link_info.busytraffic)
484 return;
485
486
487 if (mac->cnt_after_linked < 5)
488 return;
489
490 if (mac->opmode == NL80211_IFTYPE_ADHOC)
491 return;
492
493 if (mac->link_state != MAC80211_LINKED)
494 return;
495
496 mutex_lock(&rtlpriv->locks.ps_mutex);
497
498
499 if (mac->cnt_after_linked >= 2) {
500 if (ppsc->dot11_psmode == EACTIVE) {
501 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
502 "Enter 802.11 power save mode...\n");
503
504 rtl_lps_set_psmode(hw, EAUTOPS);
505 }
506 }
507
508 mutex_unlock(&rtlpriv->locks.ps_mutex);
509}
510
511
512void rtl_lps_leave(struct ieee80211_hw *hw)
513{
514 struct rtl_priv *rtlpriv = rtl_priv(hw);
515 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
516 struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
517
518 mutex_lock(&rtlpriv->locks.ps_mutex);
519
520 if (ppsc->fwctrl_lps) {
521 if (ppsc->dot11_psmode != EACTIVE) {
522
523
524 rtlpriv->cfg->ops->enable_interrupt(hw);
525
526 if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
527 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
528 rtlhal->interface == INTF_PCI) {
529 rtlpriv->intf_ops->disable_aspm(hw);
530 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
531 }
532
533 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
534 "Busy Traffic,Leave 802.11 power save..\n");
535
536 rtl_lps_set_psmode(hw, EACTIVE);
537 }
538 }
539 mutex_unlock(&rtlpriv->locks.ps_mutex);
540}
541
542
543void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
544{
545 struct rtl_priv *rtlpriv = rtl_priv(hw);
546 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
547 struct ieee80211_hdr *hdr = data;
548 struct ieee80211_tim_ie *tim_ie;
549 u8 *tim;
550 u8 tim_len;
551 bool u_buffed;
552 bool m_buffed;
553
554 if (mac->opmode != NL80211_IFTYPE_STATION)
555 return;
556
557 if (!rtlpriv->psc.swctrl_lps)
558 return;
559
560 if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
561 return;
562
563 if (!rtlpriv->psc.sw_ps_enabled)
564 return;
565
566 if (rtlpriv->psc.fwctrl_lps)
567 return;
568
569 if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
570 return;
571
572
573 if (!ieee80211_is_beacon(hdr->frame_control))
574 return;
575
576
577 if (len <= 40 + FCS_LEN)
578 return;
579
580
581 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
582 return;
583
584 rtlpriv->psc.last_beacon = jiffies;
585
586 tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
587 if (!tim)
588 return;
589
590 if (tim[1] < sizeof(*tim_ie))
591 return;
592
593 tim_len = tim[1];
594 tim_ie = (struct ieee80211_tim_ie *) &tim[2];
595
596 if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
597 rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
598
599
600
601
602 u_buffed = ieee80211_check_tim(tim_ie, tim_len,
603 rtlpriv->mac80211.assoc_id);
604
605
606 m_buffed = tim_ie->bitmap_ctrl & 0x01;
607 rtlpriv->psc.multi_buffered = m_buffed;
608
609
610
611
612 if (!m_buffed) {
613
614
615 queue_delayed_work(rtlpriv->works.rtl_wq,
616 &rtlpriv->works.ps_work, MSECS(5));
617 } else {
618 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
619 "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
620 }
621}
622EXPORT_SYMBOL_GPL(rtl_swlps_beacon);
623
624void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
625{
626 struct rtl_priv *rtlpriv = rtl_priv(hw);
627 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
628 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
629
630 if (!rtlpriv->psc.swctrl_lps)
631 return;
632 if (mac->link_state != MAC80211_LINKED)
633 return;
634
635 if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
636 RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
637 rtlpriv->intf_ops->disable_aspm(hw);
638 RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
639 }
640
641 mutex_lock(&rtlpriv->locks.ps_mutex);
642 rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
643 mutex_unlock(&rtlpriv->locks.ps_mutex);
644}
645
646void rtl_swlps_rfon_wq_callback(void *data)
647{
648 struct rtl_works *rtlworks =
649 container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
650 struct ieee80211_hw *hw = rtlworks->hw;
651
652 rtl_swlps_rf_awake(hw);
653}
654
655void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
656{
657 struct rtl_priv *rtlpriv = rtl_priv(hw);
658 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
659 struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
660 u8 sleep_intv;
661
662 if (!rtlpriv->psc.sw_ps_enabled)
663 return;
664
665 if ((rtlpriv->sec.being_setkey) ||
666 (mac->opmode == NL80211_IFTYPE_ADHOC))
667 return;
668
669
670 if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
671 return;
672
673 if (rtlpriv->link_info.busytraffic)
674 return;
675
676 mutex_lock(&rtlpriv->locks.ps_mutex);
677 rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
678 mutex_unlock(&rtlpriv->locks.ps_mutex);
679
680 if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
681 !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
682 rtlpriv->intf_ops->enable_aspm(hw);
683 RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
684 }
685
686
687
688
689
690
691
692 if (rtlpriv->psc.dtim_counter == 0) {
693 if (hw->conf.ps_dtim_period == 1)
694 sleep_intv = hw->conf.ps_dtim_period * 2;
695 else
696 sleep_intv = hw->conf.ps_dtim_period;
697 } else {
698 sleep_intv = rtlpriv->psc.dtim_counter;
699 }
700
701 if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
702 sleep_intv = MAX_SW_LPS_SLEEP_INTV;
703
704
705
706
707 RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
708 "dtim_counter:%x will sleep :%d beacon_intv\n",
709 rtlpriv->psc.dtim_counter, sleep_intv);
710
711
712 queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
713 MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
714}
715
716void rtl_lps_change_work_callback(struct work_struct *work)
717{
718 struct rtl_works *rtlworks =
719 container_of(work, struct rtl_works, lps_change_work);
720 struct ieee80211_hw *hw = rtlworks->hw;
721 struct rtl_priv *rtlpriv = rtl_priv(hw);
722
723 if (rtlpriv->enter_ps)
724 rtl_lps_enter(hw);
725 else
726 rtl_lps_leave(hw);
727}
728EXPORT_SYMBOL_GPL(rtl_lps_change_work_callback);
729
730void rtl_swlps_wq_callback(void *data)
731{
732 struct rtl_works *rtlworks = container_of_dwork_rtl(data,
733 struct rtl_works,
734 ps_work);
735 struct ieee80211_hw *hw = rtlworks->hw;
736 struct rtl_priv *rtlpriv = rtl_priv(hw);
737 bool ps = false;
738
739 ps = (hw->conf.flags & IEEE80211_CONF_PS);
740
741
742 if (rtlpriv->psc.state_inap) {
743 rtl_swlps_rf_sleep(hw);
744
745 if (rtlpriv->psc.state && !ps) {
746 rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
747 rtlpriv->psc.last_action);
748 }
749
750 if (ps)
751 rtlpriv->psc.last_slept = jiffies;
752
753 rtlpriv->psc.last_action = jiffies;
754 rtlpriv->psc.state = ps;
755 }
756}
757
758static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
759 unsigned int len)
760{
761 struct rtl_priv *rtlpriv = rtl_priv(hw);
762 struct ieee80211_mgmt *mgmt = data;
763 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
764 u8 *pos, *end, *ie;
765 u16 noa_len;
766 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
767 u8 noa_num, index, i, noa_index = 0;
768 bool find_p2p_ie = false , find_p2p_ps_ie = false;
769 pos = (u8 *)mgmt->u.beacon.variable;
770 end = data + len;
771 ie = NULL;
772
773 while (pos + 1 < end) {
774 if (pos + 2 + pos[1] > end)
775 return;
776
777 if (pos[0] == 221 && pos[1] > 4) {
778 if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
779 ie = pos + 2+4;
780 break;
781 }
782 }
783 pos += 2 + pos[1];
784 }
785
786 if (ie == NULL)
787 return;
788 find_p2p_ie = true;
789
790 while (ie + 1 < end) {
791 noa_len = READEF2BYTE((__le16 *)&ie[1]);
792 if (ie + 3 + ie[1] > end)
793 return;
794
795 if (ie[0] == 12) {
796 find_p2p_ps_ie = true;
797 if ((noa_len - 2) % 13 != 0) {
798 RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
799 "P2P notice of absence: invalid length.%d\n",
800 noa_len);
801 return;
802 } else {
803 noa_num = (noa_len - 2) / 13;
804 }
805 noa_index = ie[3];
806 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
807 P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
808 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
809 "update NOA ie.\n");
810 p2pinfo->noa_index = noa_index;
811 p2pinfo->opp_ps = (ie[4] >> 7);
812 p2pinfo->ctwindow = ie[4] & 0x7F;
813 p2pinfo->noa_num = noa_num;
814 index = 5;
815 for (i = 0; i < noa_num; i++) {
816 p2pinfo->noa_count_type[i] =
817 READEF1BYTE(ie+index);
818 index += 1;
819 p2pinfo->noa_duration[i] =
820 READEF4BYTE((__le32 *)ie+index);
821 index += 4;
822 p2pinfo->noa_interval[i] =
823 READEF4BYTE((__le32 *)ie+index);
824 index += 4;
825 p2pinfo->noa_start_time[i] =
826 READEF4BYTE((__le32 *)ie+index);
827 index += 4;
828 }
829
830 if (p2pinfo->opp_ps == 1) {
831 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
832
833
834
835 if (rtlpriv->psc.fw_current_inpsmode)
836 rtl_p2p_ps_cmd(hw,
837 P2P_PS_ENABLE);
838 } else if (p2pinfo->noa_num > 0) {
839 p2pinfo->p2p_ps_mode = P2P_PS_NOA;
840 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
841 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
842 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
843 }
844 }
845 break;
846 }
847 ie += 3 + noa_len;
848 }
849
850 if (find_p2p_ie == true) {
851 if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
852 (find_p2p_ps_ie == false))
853 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
854 }
855}
856
857static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
858 unsigned int len)
859{
860 struct rtl_priv *rtlpriv = rtl_priv(hw);
861 struct ieee80211_mgmt *mgmt = data;
862 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
863 u8 noa_num, index, i, noa_index = 0;
864 u8 *pos, *end, *ie;
865 u16 noa_len;
866 static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
867
868 pos = (u8 *)&mgmt->u.action.category;
869 end = data + len;
870 ie = NULL;
871
872 if (pos[0] == 0x7f) {
873 if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
874 ie = pos + 3+4;
875 }
876
877 if (ie == NULL)
878 return;
879
880 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n");
881
882 while (ie + 1 < end) {
883 noa_len = READEF2BYTE((__le16 *)&ie[1]);
884 if (ie + 3 + ie[1] > end)
885 return;
886
887 if (ie[0] == 12) {
888 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n");
889 RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
890 ie, noa_len);
891 if ((noa_len - 2) % 13 != 0) {
892 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
893 "P2P notice of absence: invalid length.%d\n",
894 noa_len);
895 return;
896 } else {
897 noa_num = (noa_len - 2) / 13;
898 }
899 noa_index = ie[3];
900 if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
901 P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
902 p2pinfo->noa_index = noa_index;
903 p2pinfo->opp_ps = (ie[4] >> 7);
904 p2pinfo->ctwindow = ie[4] & 0x7F;
905 p2pinfo->noa_num = noa_num;
906 index = 5;
907 for (i = 0; i < noa_num; i++) {
908 p2pinfo->noa_count_type[i] =
909 READEF1BYTE(ie+index);
910 index += 1;
911 p2pinfo->noa_duration[i] =
912 READEF4BYTE((__le32 *)ie+index);
913 index += 4;
914 p2pinfo->noa_interval[i] =
915 READEF4BYTE((__le32 *)ie+index);
916 index += 4;
917 p2pinfo->noa_start_time[i] =
918 READEF4BYTE((__le32 *)ie+index);
919 index += 4;
920 }
921
922 if (p2pinfo->opp_ps == 1) {
923 p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
924
925
926
927 if (rtlpriv->psc.fw_current_inpsmode)
928 rtl_p2p_ps_cmd(hw,
929 P2P_PS_ENABLE);
930 } else if (p2pinfo->noa_num > 0) {
931 p2pinfo->p2p_ps_mode = P2P_PS_NOA;
932 rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
933 } else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
934 rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
935 }
936 }
937 break;
938 }
939 ie += 3 + noa_len;
940 }
941}
942
943void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
944{
945 struct rtl_priv *rtlpriv = rtl_priv(hw);
946 struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
947 struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
948
949 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n", p2p_ps_state);
950 switch (p2p_ps_state) {
951 case P2P_PS_DISABLE:
952 p2pinfo->p2p_ps_state = p2p_ps_state;
953 rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
954 &p2p_ps_state);
955
956 p2pinfo->noa_index = 0;
957 p2pinfo->ctwindow = 0;
958 p2pinfo->opp_ps = 0;
959 p2pinfo->noa_num = 0;
960 p2pinfo->p2p_ps_mode = P2P_PS_NONE;
961 if (rtlps->fw_current_inpsmode == true) {
962 if (rtlps->smart_ps == 0) {
963 rtlps->smart_ps = 2;
964 rtlpriv->cfg->ops->set_hw_reg(hw,
965 HW_VAR_H2C_FW_PWRMODE,
966 &rtlps->pwr_mode);
967 }
968 }
969 break;
970 case P2P_PS_ENABLE:
971 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
972 p2pinfo->p2p_ps_state = p2p_ps_state;
973
974 if (p2pinfo->ctwindow > 0) {
975 if (rtlps->smart_ps != 0) {
976 rtlps->smart_ps = 0;
977 rtlpriv->cfg->ops->set_hw_reg(hw,
978 HW_VAR_H2C_FW_PWRMODE,
979 &rtlps->pwr_mode);
980 }
981 }
982 rtlpriv->cfg->ops->set_hw_reg(hw,
983 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
984 &p2p_ps_state);
985 }
986 break;
987 case P2P_PS_SCAN:
988 case P2P_PS_SCAN_DONE:
989 case P2P_PS_ALLSTASLEEP:
990 if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
991 p2pinfo->p2p_ps_state = p2p_ps_state;
992 rtlpriv->cfg->ops->set_hw_reg(hw,
993 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
994 &p2p_ps_state);
995 }
996 break;
997 default:
998 break;
999 }
1000 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
1001 "ctwindow %x oppps %x\n", p2pinfo->ctwindow, p2pinfo->opp_ps);
1002 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
1003 "count %x duration %x index %x interval %x start time %x noa num %x\n",
1004 p2pinfo->noa_count_type[0], p2pinfo->noa_duration[0],
1005 p2pinfo->noa_index, p2pinfo->noa_interval[0],
1006 p2pinfo->noa_start_time[0], p2pinfo->noa_num);
1007 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n");
1008}
1009
1010void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
1011{
1012 struct rtl_priv *rtlpriv = rtl_priv(hw);
1013 struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
1014 struct ieee80211_hdr *hdr = data;
1015
1016 if (!mac->p2p)
1017 return;
1018 if (mac->link_state != MAC80211_LINKED)
1019 return;
1020
1021 if (len <= 40 + FCS_LEN)
1022 return;
1023
1024
1025 if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
1026 return;
1027
1028
1029 if (!(ieee80211_is_beacon(hdr->frame_control) ||
1030 ieee80211_is_probe_resp(hdr->frame_control) ||
1031 ieee80211_is_action(hdr->frame_control)))
1032 return;
1033
1034 if (ieee80211_is_action(hdr->frame_control))
1035 rtl_p2p_action_ie(hw, data, len - FCS_LEN);
1036 else
1037 rtl_p2p_noa_ie(hw, data, len - FCS_LEN);
1038}
1039EXPORT_SYMBOL_GPL(rtl_p2p_info);
1040
