cw1200: AP PS refactoring.

 * buffered_multicasts_lock was renamed to ps_state_lock.
   Previous name was quite confusive.
 * Per-STA rx_queue was created for early RX-ed frames.
   Not that we really need these frames, but PM status they are
   holding is important.
 * priv->tx_suspend_mask was removed, driver is not using it.
   It was intended for UAPSD and is not needed in current
   implementation on mac80211.
 * Fix: cw1200_queue_unlock() was not called from cw1200_queue_clear()
   when queue was internally locked.

ST-Ericsson ID: 360749

Change-Id: I61346db485d34f761d80af786b716d8c73e8b600
Signed-off-by: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33541
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

10 files changed, 143 insertions, 152 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 5e7177f85cf..5a75ac508d1 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -29,6 +29,10 @@ static int cw1200_start_ap(struct cw1200_common *priv);
 static int cw1200_update_beaconing(struct cw1200_common *priv);
 static int cw1200_enable_beaconing(struct cw1200_common *priv,
 				   bool enable);
+static void __cw1200_sta_notify(struct ieee80211_hw *dev,
+				struct ieee80211_vif *vif,
+				enum sta_notify_cmd notify_cmd,
+				struct ieee80211_sta *sta);
 
 /* ******************************************************************** */
 /* AP API								*/
@@ -39,22 +43,26 @@ int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct cw1200_common *priv = hw->priv;
 	struct cw1200_sta_priv *sta_priv =
 			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct cw1200_link_entry *entry;
+	struct sk_buff *skb;
 
 	if (priv->mode != NL80211_IFTYPE_AP)
 		return 0;
 
 	sta_priv->link_id = cw1200_find_link_id(priv, sta->addr);
-	if (!sta_priv->link_id) {
+	if (WARN_ON(!sta_priv->link_id)) {
+		/* Impossible error */
 		wiphy_info(priv->hw->wiphy,
 			"[AP] No more link IDs available.\n");
 		return -ENOENT;
 	}
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
-	priv->link_id_db[sta_priv->link_id - 1].status = CW1200_LINK_HARD;
-	if (priv->link_id_db[sta_priv->link_id - 1].ps)
-		ieee80211_sta_ps_transition(sta, true);
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	entry->status = CW1200_LINK_HARD;
+	while ((skb = skb_dequeue(&entry->rx_queue)))
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	spin_unlock_bh(&priv->ps_state_lock);
 	return 0;
 }
 
@@ -64,19 +72,24 @@ int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct cw1200_common *priv = hw->priv;
 	struct cw1200_sta_priv *sta_priv =
 			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct cw1200_link_entry *entry;
 
 	if (priv->mode != NL80211_IFTYPE_AP || !sta_priv->link_id)
 		return 0;
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
-	priv->link_id_db[sta_priv->link_id - 1].status = CW1200_LINK_SOFT;
-	priv->link_id_db[sta_priv->link_id - 1].timestamp = jiffies;
+	/* HACK! To be removed when accurate TX ststus
+	 * reporting for dropped frames is implemented. */
+	ieee80211_sta_eosp_irqsafe(sta);
+
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	entry->status = CW1200_LINK_SOFT;
+	entry->timestamp = jiffies;
 	if (!delayed_work_pending(&priv->link_id_gc_work))
 		queue_delayed_work(priv->workqueue,
 				&priv->link_id_gc_work,
 				CW1200_LINK_ID_GC_TIMEOUT);
-	priv->link_id_db[sta_priv->link_id - 1].ps = false;
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 	return 0;
 }
 
@@ -89,45 +102,44 @@ static void __cw1200_sta_notify(struct ieee80211_hw *dev,
 	struct cw1200_sta_priv *sta_priv =
 		(struct cw1200_sta_priv *)&sta->drv_priv;
 	u32 bit = BIT(sta_priv->link_id);
+	u32 prev = priv->sta_asleep_mask & bit;
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
 	switch (notify_cmd) {
 		case STA_NOTIFY_SLEEP:
-			if (priv->buffered_multicasts &&
+			if (!prev) {
+				if (priv->buffered_multicasts &&
 					!priv->sta_asleep_mask)
 				queue_work(priv->workqueue,
 					&priv->multicast_start_work);
-			priv->sta_asleep_mask |= bit;
+				priv->sta_asleep_mask |= bit;
+			}
 			break;
 		case STA_NOTIFY_AWAKE:
-			priv->sta_asleep_mask &= ~bit;
-			if (priv->tx_multicast &&
-					!priv->sta_asleep_mask)
-				queue_work(priv->workqueue,
-					&priv->multicast_stop_work);
-			cw1200_bh_wakeup(priv);
+			if (prev) {
+				priv->sta_asleep_mask &= ~bit;
+				priv->pspoll_mask &= ~bit;
+				if (priv->tx_multicast &&
+						!priv->sta_asleep_mask)
+					queue_work(priv->workqueue,
+						&priv->multicast_stop_work);
+				cw1200_bh_wakeup(priv);
+			}
 			break;
 	}
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
 }
 
-static void __cw1200_ps_notify(struct cw1200_common *priv,
-			       struct ieee80211_sta *sta,
-			       int link_id, bool ps)
+void cw1200_sta_notify(struct ieee80211_hw *dev,
+		       struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta)
 {
-	txrx_printk(KERN_DEBUG "%s for LinkId: %d. Suspend: %.8X\n",
-			ps ? "Stop" : "Start",
-			link_id, priv->tx_suspend_mask[0]);
-
-	priv->link_id_db[link_id - 1].ps = ps;
-	if (sta) {
-		__cw1200_sta_notify(priv->hw, priv->vif,
-			ps ? STA_NOTIFY_SLEEP : STA_NOTIFY_AWAKE, sta);
-		ieee80211_sta_ps_transition_ni(sta, ps);
-	}
+	struct cw1200_common *priv = dev->priv;
+	spin_lock_bh(&priv->ps_state_lock);
+	__cw1200_sta_notify(dev, vif, notify_cmd, sta);
+	spin_unlock_bh(&priv->ps_state_lock);
 }
 
-void cw1200_ps_notify(struct cw1200_common *priv,
+static void cw1200_ps_notify(struct cw1200_common *priv,
 		      int link_id, bool ps)
 {
 	struct ieee80211_sta *sta;
@@ -135,13 +147,17 @@ void cw1200_ps_notify(struct cw1200_common *priv,
 	if (!link_id || link_id > CW1200_MAX_STA_IN_AP_MODE)
 		return;
 
-	if (!!ps == !!priv->link_id_db[link_id - 1].ps)
-		return;
+	txrx_printk(KERN_DEBUG "%s for LinkId: %d. STAs asleep: %.8X\n",
+			ps ? "Stop" : "Start",
+			link_id, priv->sta_asleep_mask);
 
 	rcu_read_lock();
 	sta = ieee80211_find_sta(priv->vif,
 			priv->link_id_db[link_id - 1].mac);
-	__cw1200_ps_notify(priv, sta, link_id, ps);
+	if (sta) {
+		__cw1200_sta_notify(priv->hw, priv->vif,
+			ps ? STA_NOTIFY_SLEEP : STA_NOTIFY_AWAKE, sta);
+	}
 	rcu_read_unlock();
 }
 
@@ -594,12 +610,12 @@ void cw1200_mcast_timeout(unsigned long arg)
 	struct cw1200_common *priv =
 		(struct cw1200_common *)arg;
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
+	spin_lock_bh(&priv->ps_state_lock);
 	priv->tx_multicast = priv->aid0_bit_set &&
 			priv->buffered_multicasts;
 	if (priv->tx_multicast)
 		cw1200_bh_wakeup(priv);
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 }
 
 int cw1200_ampdu_action(struct ieee80211_hw *hw,
@@ -620,18 +636,6 @@ int cw1200_ampdu_action(struct ieee80211_hw *hw,
 void cw1200_suspend_resume(struct cw1200_common *priv,
 			  struct wsm_suspend_resume *arg)
 {
-	int queue = BIT(wsm_queue_id_to_linux(arg->queue));
-	u32 unicast = BIT(arg->link_id);
-	u32 wakeup_required = 0;
-	u32 set = 0;
-	u32 clear;
-	u32 tx_suspend_mask;
-	bool cancel_tmo = false;
-	int i;
-
-	if (!arg->link_id) /* For all links */
-		unicast = BIT(CW1200_MAX_STA_IN_AP_MODE + 1) - 2;
-
 	/* if () is intendend to protect against spam. FW sends
 	 * "start multicast" request on every DTIM. */
 	if (arg->stop || !arg->multicast || priv->buffered_multicasts)
@@ -640,7 +644,8 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 				arg->multicast ? "broadcast" : "unicast");
 
 	if (arg->multicast) {
-		spin_lock_bh(&priv->buffered_multicasts_lock);
+		bool cancel_tmo = false;
+		spin_lock_bh(&priv->ps_state_lock);
 		if (arg->stop) {
 			priv->tx_multicast = false;
 		} else {
@@ -658,44 +663,19 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 				cw1200_bh_wakeup(priv);
 			}
 		}
-		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		spin_unlock_bh(&priv->ps_state_lock);
 		if (cancel_tmo)
 			del_timer_sync(&priv->mcast_timeout);
 	} else {
-		if (arg->stop)
-			set = unicast;
-		else
-			set = 0;
-
-		clear = set ^ unicast;
-
-		/* TODO: if (!priv->uapsd) */
-		queue = 0x0F;
-
-		spin_lock_bh(&priv->buffered_multicasts_lock);
-		priv->sta_asleep_mask |= set;
-		for (i = 0; i < 4; ++i) {
-			if (!(queue & BIT(i)))
-				continue;
-
-			tx_suspend_mask = priv->tx_suspend_mask[i];
-			priv->tx_suspend_mask[i] =
-				(tx_suspend_mask & ~clear) | set;
-
-			wakeup_required = wakeup_required ||
-				cw1200_queue_get_num_queued(
-					&priv->tx_queue[i],
-					tx_suspend_mask & clear);
-		}
-		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		spin_lock_bh(&priv->ps_state_lock);
 		cw1200_ps_notify(priv, arg->link_id, arg->stop);
+		spin_unlock_bh(&priv->ps_state_lock);
+		if (!arg->stop)
+			cw1200_bh_wakeup(priv);
 	}
-	if (wakeup_required)
-		cw1200_bh_wakeup(priv);
 	return;
 }
 
-
 /* ******************************************************************** */
 /* AP privates								*/
 
@@ -873,7 +853,7 @@ static int cw1200_update_beaconing(struct cw1200_common *priv)
 int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
 {
 	int i, ret = 0;
-	spin_lock_bh(&priv->buffered_multicasts_lock);
+	spin_lock_bh(&priv->ps_state_lock);
 	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
 		if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
 				priv->link_id_db[i].status) {
@@ -882,7 +862,7 @@ int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
 			break;
 		}
 	}
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 	return ret;
 }
 
@@ -892,7 +872,7 @@ int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
 	unsigned long max_inactivity = 0;
 	unsigned long now = jiffies;
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
+	spin_lock_bh(&priv->ps_state_lock);
 	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
 		if (!priv->link_id_db[i].status) {
 			ret = i + 1;
@@ -909,11 +889,13 @@ int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
 		}
 	}
 	if (ret) {
+		struct cw1200_link_entry *entry = &priv->link_id_db[ret - 1];
 		ap_printk(KERN_DEBUG "[AP] STA added, link_id: %d\n",
 			ret);
-		priv->link_id_db[ret - 1].status = CW1200_LINK_RESERVE;
-		memcpy(&priv->link_id_db[ret - 1].mac, mac, ETH_ALEN);
-		memset(&priv->link_id_db[ret - 1].buffered, 0, CW1200_MAX_TID);
+		entry->status = CW1200_LINK_RESERVE;
+		memcpy(&entry->mac, mac, ETH_ALEN);
+		memset(&entry->buffered, 0, CW1200_MAX_TID);
+		skb_queue_head_init(&entry->rx_queue);
 		wsm_lock_tx_async(priv);
 		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
 			wsm_unlock_tx(priv);
@@ -922,7 +904,7 @@ int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
 			"[AP] Early: no more link IDs available.\n");
 	}
 
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 	return ret;
 }
 
@@ -950,13 +932,13 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 	long ttl;
 	bool need_reset;
 	u32 mask;
-	int i, j;
+	int i;
 
 	if (priv->join_status != CW1200_JOIN_STATUS_AP)
 		return;
 
 	wsm_lock_tx(priv);
-	spin_lock_bh(&priv->buffered_multicasts_lock);
+	spin_lock_bh(&priv->ps_state_lock);
 	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
 		need_reset = false;
 		mask = BIT(i + 1);
@@ -965,8 +947,7 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 			 !(priv->link_id_map & mask))) {
 			if (priv->link_id_map & mask) {
 				priv->sta_asleep_mask &= ~mask;
-				for (j = 0; j < 4; ++j)
-					priv->tx_suspend_mask[j] &= ~mask;
+				priv->pspoll_mask &= ~mask;
 				need_reset = true;
 			}
 			priv->link_id_map |= mask;
@@ -974,7 +955,7 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 				priv->link_id_db[i].status = CW1200_LINK_SOFT;
 			memcpy(map_link.mac_addr, priv->link_id_db[i].mac,
 					ETH_ALEN);
-			spin_unlock_bh(&priv->buffered_multicasts_lock);
+			spin_unlock_bh(&priv->ps_state_lock);
 			if (need_reset) {
 				reset.link_id = i + 1;
 				WARN_ON(wsm_reset(priv, &reset));
@@ -982,7 +963,7 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 			map_link.link_id = i + 1;
 			WARN_ON(wsm_map_link(priv, &map_link));
 			next_gc = min(next_gc, CW1200_LINK_ID_GC_TIMEOUT);
-			spin_lock_bh(&priv->buffered_multicasts_lock);
+			spin_lock_bh(&priv->ps_state_lock);
 		} else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
 			ttl = priv->link_id_db[i].timestamp - now +
 					CW1200_LINK_ID_GC_TIMEOUT;
@@ -991,22 +972,23 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 				priv->link_id_db[i].status = CW1200_LINK_OFF;
 				priv->link_id_map &= ~mask;
 				priv->sta_asleep_mask &= ~mask;
-				for (j = 0; j < 4; ++j)
-					priv->tx_suspend_mask[j] &= ~mask;
+				priv->pspoll_mask &= ~mask;
 				memset(map_link.mac_addr, 0, ETH_ALEN);
-				spin_unlock_bh(&priv->buffered_multicasts_lock);
+				spin_unlock_bh(&priv->ps_state_lock);
 				reset.link_id = i + 1;
 				WARN_ON(wsm_reset(priv, &reset));
-				spin_lock_bh(&priv->buffered_multicasts_lock);
+				spin_lock_bh(&priv->ps_state_lock);
 			} else {
 				next_gc = min(next_gc, (unsigned long)ttl);
 			}
 		}
-		if (need_reset)
+		if (need_reset) {
+			skb_queue_purge(&priv->link_id_db[i].rx_queue);
 			ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
 					reset.link_id);
+		}
 	}
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 	if (next_gc != -1)
 		queue_delayed_work(priv->workqueue,
 				&priv->link_id_gc_work, next_gc);
diff --git a/drivers/staging/cw1200/ap.h b/drivers/staging/cw1200/ap.h
index 698c01d05ca..a9e85bd5516 100644
--- a/drivers/staging/cw1200/ap.h
+++ b/drivers/staging/cw1200/ap.h
@@ -18,6 +18,9 @@ int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   struct ieee80211_sta *sta);
 int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		      struct ieee80211_sta *sta);
+void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta);
 void cw1200_bss_info_changed(struct ieee80211_hw *dev,
 			     struct ieee80211_vif *vif,
 			     struct ieee80211_bss_conf *info,
@@ -38,7 +41,5 @@ int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac);
 int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac);
 void cw1200_link_id_work(struct work_struct *work);
 void cw1200_link_id_gc_work(struct work_struct *work);
-void cw1200_ps_notify(struct cw1200_common *priv,
-		      int link_id, bool ps);
 
 #endif
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 90df67952f5..ae08d82fe19 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -72,7 +72,7 @@ struct cw1200_link_entry {
 	enum cw1200_link_status		status;
 	u8				mac[ETH_ALEN];
 	u8				buffered[CW1200_MAX_TID];
-	bool				ps;
+	struct sk_buff_head		rx_queue;
 };
 
 struct cw1200_common {
@@ -198,11 +198,10 @@ struct cw1200_common {
 	struct cw1200_link_entry link_id_db[CW1200_MAX_STA_IN_AP_MODE];
 	struct work_struct	link_id_work;
 	struct delayed_work	link_id_gc_work;
-	u32			tx_suspend_mask[4];
 	u32			sta_asleep_mask;
 	u32			pspoll_mask;
 	bool			aid0_bit_set;
-	spinlock_t		buffered_multicasts_lock;
+	spinlock_t		ps_state_lock;
 	bool			buffered_multicasts;
 	bool			tx_multicast;
 	struct work_struct	set_tim_work;
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index e1fc0a8a748..db927620f26 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -211,11 +211,7 @@ static int cw1200_status_show(struct seq_file *seq, void *v)
 
 	seq_puts(seq, "\n");
 	for (i = 0; i < 4; ++i) {
-		char buf[32];
-		snprintf(buf, sizeof(buf), "TX lock(%d): ", i);
 		cw1200_queue_status_show(seq, &priv->tx_queue[i]);
-		cw1200_debug_print_map(seq, priv, buf,
-			priv->tx_suspend_mask[i]);
 		seq_puts(seq, "\n");
 	}
 
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index c09b44fb367..d2f6cee19ee 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -207,6 +207,7 @@ static const struct ieee80211_ops cw1200_ops = {
 	.tx			= cw1200_tx,
 	.hw_scan		= cw1200_hw_scan,
 	.set_tim		= cw1200_set_tim,
+	.sta_notify		= cw1200_sta_notify,
 	.sta_add		= cw1200_sta_add,
 	.sta_remove		= cw1200_sta_remove,
 	.set_key		= cw1200_set_key,
@@ -325,7 +326,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	INIT_DELAYED_WORK(&priv->connection_loss_work,
 			  cw1200_connection_loss_work);
 	INIT_WORK(&priv->tx_failure_work, cw1200_tx_failure_work);
-	spin_lock_init(&priv->buffered_multicasts_lock);
+	spin_lock_init(&priv->ps_state_lock);
 	INIT_WORK(&priv->set_tim_work, cw1200_set_tim_work);
 	INIT_WORK(&priv->multicast_start_work, cw1200_multicast_start_work);
 	INIT_WORK(&priv->multicast_stop_work, cw1200_multicast_stop_work);
@@ -350,7 +351,8 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 		if (unlikely(cw1200_queue_init(&priv->tx_queue[i],
 				&priv->tx_queue_stats, i, 16))) {
 			for (; i > 0; i--)
-				cw1200_queue_deinit(&priv->tx_queue[i - 1]);
+				cw1200_queue_deinit(&priv->tx_queue[i - 1],
+						priv);
 			cw1200_queue_stats_deinit(&priv->tx_queue_stats);
 			ieee80211_free_hw(hw);
 			return NULL;
@@ -440,7 +442,7 @@ void cw1200_unregister_common(struct ieee80211_hw *dev)
 	}
 
 	for (i = 0; i < 4; ++i)
-		cw1200_queue_deinit(&priv->tx_queue[i]);
+		cw1200_queue_deinit(&priv->tx_queue[i], priv);
 	cw1200_queue_stats_deinit(&priv->tx_queue_stats);
 	cw1200_pm_deinit(&priv->pm_state);
 }
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index c667c433766..83b294dc1da 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -116,7 +116,7 @@ int cw1200_queue_init(struct cw1200_queue *queue,
 	return 0;
 }
 
-int cw1200_queue_clear(struct cw1200_queue *queue)
+int cw1200_queue_clear(struct cw1200_queue *queue, struct cw1200_common *priv)
 {
 	int i;
 	struct cw1200_queue_stats *stats = queue->stats;
@@ -145,6 +145,10 @@ int cw1200_queue_clear(struct cw1200_queue *queue)
 		queue->link_map_cache[i] = 0;
 	}
 	spin_unlock_bh(&stats->lock);
+	if (unlikely(queue->overfull)) {
+		queue->overfull = false;
+		__cw1200_queue_unlock(queue, priv);
+	}
 	spin_unlock_bh(&queue->lock);
 	wake_up(&stats->wait_link_id_empty);
 	return 0;
@@ -156,9 +160,10 @@ void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
 	stats->link_map_cache = NULL;
 }
 
-void cw1200_queue_deinit(struct cw1200_queue *queue)
+void cw1200_queue_deinit(struct cw1200_queue *queue,
+			 struct cw1200_common *priv)
 {
-	cw1200_queue_clear(queue);
+	cw1200_queue_clear(queue, priv);
 	INIT_LIST_HEAD(&queue->free_pool);
 	kfree(queue->pool);
 	kfree(queue->link_map_cache);
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index 195d6acb6ec..502496b142b 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -60,9 +60,10 @@ int cw1200_queue_init(struct cw1200_queue *queue,
 		      struct cw1200_queue_stats *stats,
 		      u8 queue_id,
 		      size_t capacity);
-int cw1200_queue_clear(struct cw1200_queue *queue);
+int cw1200_queue_clear(struct cw1200_queue *queue, struct cw1200_common *priv);
 void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats);
-void cw1200_queue_deinit(struct cw1200_queue *queue);
+void cw1200_queue_deinit(struct cw1200_queue *queue,
+			 struct cw1200_common *priv);
 
 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 				   u32 link_id_map);
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index ecfdc7562f3..b6ea70f3418 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -136,7 +136,7 @@ void cw1200_stop(struct ieee80211_hw *dev)
 	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
 
 	for (i = 0; i < 4; i++)
-		cw1200_queue_clear(&priv->tx_queue[i]);
+		cw1200_queue_clear(&priv->tx_queue[i], priv);
 
 	/* HACK! */
 	if (atomic_xchg(&priv->tx_lock, 1) != 1)
@@ -215,8 +215,6 @@ void cw1200_remove_interface(struct ieee80211_hw *dev,
 		}
 		memset(priv->link_id_db, 0,
 				sizeof(priv->link_id_db));
-		memset(priv->tx_suspend_mask, 0,
-				sizeof(priv->tx_suspend_mask));
 		priv->sta_asleep_mask = 0;
 		priv->enable_beacon = false;
 		priv->tx_multicast = false;
@@ -762,7 +760,7 @@ int __cw1200_flush(struct cw1200_common *priv, bool drop)
 		 */
 		if (drop) {
 			for (i = 0; i < 4; ++i)
-				cw1200_queue_clear(&priv->tx_queue[i]);
+				cw1200_queue_clear(&priv->tx_queue[i], priv);
 		} else {
 			ret = wait_event_timeout(
 				priv->tx_queue_stats.wait_link_id_empty,
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 5a55386127c..04b7b0d091e 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -462,7 +462,6 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		(struct cw1200_sta_priv *)&tx_info->control.sta->drv_priv;
 	int link_id, raw_link_id;
 	int ret;
-	int i;
 	struct __cw1200_txpriv txpriv = {
 		.super.tid = CW1200_MAX_TID,
 	};
@@ -504,11 +503,10 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		goto err;
 
 	if (unlikely(ieee80211_is_auth(hdr->frame_control))) {
-		spin_lock_bh(&priv->buffered_multicasts_lock);
+		spin_lock_bh(&priv->ps_state_lock);
 		priv->sta_asleep_mask &= ~BIT(raw_link_id);
-		for (i = 0; i < 4; ++i)
-			priv->tx_suspend_mask[i] &= ~BIT(raw_link_id);
-		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		priv->pspoll_mask &= ~BIT(raw_link_id);
+		spin_unlock_bh(&priv->ps_state_lock);
 	} else if (ieee80211_is_data_qos(hdr->frame_control) ||
 			ieee80211_is_qos_nullfunc(hdr->frame_control)) {
 		u8 *qos = ieee80211_get_qos_ctl(hdr);
@@ -620,7 +618,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		if (cw1200_handle_action_tx(priv, skb))
 			goto drop;
 
-	spin_lock_bh(&priv->buffered_multicasts_lock);
+	spin_lock_bh(&priv->ps_state_lock);
 
 	if (link_id == CW1200_LINK_ID_AFTER_DTIM &&
 			!priv->buffered_multicasts) {
@@ -639,7 +637,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	ret = cw1200_queue_put(&priv->tx_queue[queue], priv, skb,
 			&txpriv.super);
 
-	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	spin_unlock_bh(&priv->ps_state_lock);
 
 	if (raw_link_id && !was_buffered && txpriv.super.tid < CW1200_MAX_TID)
 		ieee80211_sta_set_buffered(tx_info->control.sta,
@@ -695,7 +693,7 @@ static int cw1200_handle_pspoll(struct cw1200_common *priv,
 	struct ieee80211_pspoll *pspoll =
 		(struct ieee80211_pspoll *) skb->data;
 	int link_id = 0;
-	u32 pspoll_mask;
+	u32 pspoll_mask = 0;
 	int drop = 1;
 	int i;
 
@@ -711,17 +709,16 @@ static int cw1200_handle_pspoll(struct cw1200_common *priv,
 		sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
 		link_id = sta_priv->link_id;
 		pspoll_mask = BIT(sta_priv->link_id);
+
+		/* HACK! To be removed when accurate TX ststus
+		 * reporting for dropped frames is implemented. */
+		if (priv->pspoll_mask & pspoll_mask)
+			ieee80211_sta_eosp_irqsafe(sta);
 	}
 	rcu_read_unlock();
 	if (!link_id)
-		/* Slowpath */
-		link_id = cw1200_find_link_id(priv, pspoll->ta);
-
-	if (!link_id)
 		goto done;
 
-	pspoll_mask = BIT(link_id);
-
 	priv->pspoll_mask |= pspoll_mask;
 	drop = 0;
 
@@ -856,10 +853,10 @@ void cw1200_notify_buffered_tx(struct cw1200_common *priv,
 		buffered = priv->link_id_db
 				[link_id - 1].buffered;
 
-		spin_lock_bh(&priv->buffered_multicasts_lock);
+		spin_lock_bh(&priv->ps_state_lock);
 		if (!WARN_ON(!buffered[tid]))
 			still_buffered = --buffered[tid];
-		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		spin_unlock_bh(&priv->ps_state_lock);
 
 		if (!still_buffered && tid < CW1200_MAX_TID) {
 			hdr = (struct ieee80211_hdr *) skb->data;
@@ -880,7 +877,9 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 	struct sk_buff *skb = *skb_p;
 	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
 	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
+	struct cw1200_link_entry *entry = NULL;
 	unsigned long grace_period;
+	bool early_data = false;
 	hdr->flag = 0;
 
 	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
@@ -888,8 +887,13 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 		goto drop;
 	}
 
-	if (arg->link_id && arg->link_id <= CW1200_MAX_STA_IN_AP_MODE)
-		priv->link_id_db[arg->link_id - 1].timestamp = jiffies;
+	if (arg->link_id && arg->link_id <= CW1200_MAX_STA_IN_AP_MODE) {
+		entry =	&priv->link_id_db[arg->link_id - 1];
+		if (entry->status == CW1200_LINK_SOFT &&
+				ieee80211_is_data(frame->frame_control))
+			early_data = true;
+		entry->timestamp = jiffies;
+	}
 
 	if (unlikely(arg->status)) {
 		if (arg->status == WSM_STATUS_MICFAILURE) {
@@ -996,15 +1000,19 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 		grace_period = 1 * HZ;
 	cw1200_pm_stay_awake(&priv->pm_state, grace_period);
 
-	/* Notify driver and mac80211 about PM state */
-	cw1200_ps_notify(priv, arg->link_id,
-			ieee80211_has_pm(frame->frame_control));
-
-	/* Not that we really need _irqsafe variant here,
-	 * but it offloads realtime bh thread and improve
-	 * system performance. */
-	ieee80211_rx_irqsafe(priv->hw, skb);
+	if (unlikely(early_data)) {
+		spin_lock_bh(&priv->ps_state_lock);
+		/* Double-check status with lock held */
+		if (entry->status == CW1200_LINK_SOFT)
+			skb_queue_tail(&entry->rx_queue, skb);
+		else
+			ieee80211_rx_irqsafe(priv->hw, skb);
+		spin_unlock_bh(&priv->ps_state_lock);
+	} else {
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	}
 	*skb_p = NULL;
+
 	return;
 
 drop:
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 415fbcbee59..d0bcffa1654 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1555,7 +1555,6 @@ static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
 		tx_allowed_mask = ~priv->sta_asleep_mask;
 		tx_allowed_mask |= BIT(CW1200_LINK_ID_UAPSD);
 		if (priv->sta_asleep_mask) {
-			tx_allowed_mask |= ~priv->tx_suspend_mask[i];
 			tx_allowed_mask |= priv->pspoll_mask;
 			tx_allowed_mask &= ~BIT(CW1200_LINK_ID_AFTER_DTIM);
 		} else {
@@ -1607,7 +1606,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 			if (atomic_add_return(0, &priv->tx_lock))
 				break;
 
-			spin_lock_bh(&priv->buffered_multicasts_lock);
+			spin_lock_bh(&priv->ps_state_lock);
 
 			ret = wsm_get_tx_queue_and_mask(priv, &queue,
 					&tx_allowed_mask, &more);
@@ -1622,7 +1621,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 						&priv->multicast_stop_work);
 			}
 
-			spin_unlock_bh(&priv->buffered_multicasts_lock);
+			spin_unlock_bh(&priv->ps_state_lock);
 
 			if (ret)
 				break;