p54: re-enable power save feature

This patch re-enables p54's power save features and adds a workaround
which temporarily alters the device's power state in order to allow
ps-polls to be sent and buffered data to be retrieved during psm.

(Incorporates patch originally posted as "p54: fix beacon template dtim
IE corruption". -- JWL)

Signed-off-by: Christian Lamparter <chunkeey@web.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 42 insertions, 0 deletions

diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/p54/txrx.c
index 6426d2cae6d..01eadb1683f 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/p54/txrx.c
@@ -288,6 +288,45 @@ static int p54_rssi_to_dbm(struct p54_common *priv, int rssi)
 			 priv->rssical_db[band].add) / 4;
 }
 
+/*
+ * Even if the firmware is capable of dealing with incoming traffic,
+ * while dozing, we have to prepared in case mac80211 uses PS-POLL
+ * to retrieve outstanding frames from our AP.
+ * (see comment in net/mac80211/mlme.c @ line 1993)
+ */
+static void p54_pspoll_workaround(struct p54_common *priv, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (void *) skb->data;
+	struct ieee80211_tim_ie *tim_ie;
+	u8 *tim;
+	u8 tim_len;
+	bool new_psm;
+
+	/* only beacons have a TIM IE */
+	if (!ieee80211_is_beacon(hdr->frame_control))
+		return;
+
+	if (!priv->aid)
+		return;
+
+	/* only consider beacons from the associated BSSID */
+	if (compare_ether_addr(hdr->addr3, priv->bssid))
+		return;
+
+	tim = p54_find_ie(skb, WLAN_EID_TIM);
+	if (!tim)
+		return;
+
+	tim_len = tim[1];
+	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
+
+	new_psm = ieee80211_check_tim(tim_ie, tim_len, priv->aid);
+	if (new_psm != priv->powersave_override) {
+		priv->powersave_override = new_psm;
+		p54_set_ps(priv);
+	}
+}
+
 static int p54_rx_data(struct p54_common *priv, struct sk_buff *skb)
 {
 	struct p54_rx_data *hdr = (struct p54_rx_data *) skb->data;
@@ -340,6 +379,9 @@ static int p54_rx_data(struct p54_common *priv, struct sk_buff *skb)
 
 	skb_pull(skb, header_len);
 	skb_trim(skb, le16_to_cpu(hdr->len));
+	if (unlikely(priv->hw->conf.flags & IEEE80211_CONF_PS))
+		p54_pspoll_workaround(priv, skb);
+
 	ieee80211_rx_irqsafe(priv->hw, skb);
 
 	queue_delayed_work(priv->hw->workqueue, &priv->work,