1 files changed, 88 insertions, 0 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 7776d9f1f297..2eb5196977fd 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -141,6 +141,16 @@ static void rt2x00lib_intf_scheduled(struct work_struct *work)
 					    rt2x00dev);
 }
 
+static void rt2x00lib_autowakeup(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, autowakeup_work.work);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		ERROR(rt2x00dev, "Device failed to wakeup.\n");
+	clear_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
+}
+
 /*
  * Interrupt context handlers.
  */
@@ -416,6 +426,77 @@ void rt2x00lib_txdone_noinfo(struct queue_entry *entry, u32 status)
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_txdone_noinfo);
 
+static u8 *rt2x00lib_find_ie(u8 *data, unsigned int len, u8 ie)
+{
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	u8 *pos, *end;
+
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	while (pos < end) {
+		if (pos + 2 + pos[1] > end)
+			return NULL;
+
+		if (pos[0] == ie)
+			return pos;
+
+		pos += 2 + pos[1];
+	}
+
+	return NULL;
+}
+
+static void rt2x00lib_rxdone_check_ps(struct rt2x00_dev *rt2x00dev,
+				      struct sk_buff *skb,
+				      struct rxdone_entry_desc *rxdesc)
+{
+	struct ieee80211_hdr *hdr = (void *) skb->data;
+	struct ieee80211_tim_ie *tim_ie;
+	u8 *tim;
+	u8 tim_len;
+	bool cam;
+
+	/* If this is not a beacon, or if mac80211 has no powersaving
+	 * configured, or if the device is already in powersaving mode
+	 * we can exit now. */
+	if (likely(!ieee80211_is_beacon(hdr->frame_control) ||
+		   !(rt2x00dev->hw->conf.flags & IEEE80211_CONF_PS)))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (skb->len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (!(rxdesc->dev_flags & RXDONE_MY_BSS) ||
+	    !rt2x00dev->aid)
+		return;
+
+	rt2x00dev->last_beacon = jiffies;
+
+	tim = rt2x00lib_find_ie(skb->data, skb->len - FCS_LEN, WLAN_EID_TIM);
+	if (!tim)
+		return;
+
+	if (tim[1] < sizeof(*tim_ie))
+		return;
+
+	tim_len = tim[1];
+	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
+
+	/* Check whenever the PHY can be turned off again. */
+
+	/* 1. What about buffered unicast traffic for our AID? */
+	cam = ieee80211_check_tim(tim_ie, tim_len, rt2x00dev->aid);
+
+	/* 2. Maybe the AP wants to send multicast/broadcast data? */
+	cam |= (tim_ie->bitmap_ctrl & 0x01);
+
+	if (!cam && !test_bit(CONFIG_POWERSAVING, &rt2x00dev->flags))
+		rt2x00lib_config(rt2x00dev, &rt2x00dev->hw->conf,
+				 IEEE80211_CONF_CHANGE_PS);
+}
+
 static int rt2x00lib_rxdone_read_signal(struct rt2x00_dev *rt2x00dev,
 					struct rxdone_entry_desc *rxdesc)
 {
@@ -531,6 +612,12 @@ void rt2x00lib_rxdone(struct queue_entry *entry)
 		rxdesc.flags |= RX_FLAG_HT;
 
 	/*
+	 * Check if this is a beacon, and more frames have been
+	 * buffered while we were in powersaving mode.
+	 */
+	rt2x00lib_rxdone_check_ps(rt2x00dev, entry->skb, &rxdesc);
+
+	/*
 	 * Update extra components
 	 */
 	rt2x00link_update_stats(rt2x00dev, entry->skb, &rxdesc);
@@ -1017,6 +1104,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	}
 
 	INIT_WORK(&rt2x00dev->intf_work, rt2x00lib_intf_scheduled);
+	INIT_DELAYED_WORK(&rt2x00dev->autowakeup_work, rt2x00lib_autowakeup);
 
 	/*
 	 * Let the driver probe the device to detect the capabilities.