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