cw1200: PM state in AP mode could be out of sync.

PM state is controlled separately by firmware and driver.
Firmware does not update own PM state when STA is removed,
so PM state of the driver and firmware could be out of sync.

The patch implements resyncronization of the PM state.

ST-Ericsson ID: 354923
Change-Id: Ie2d8f54bc9d6dc1578aead31eecdb04c9ce7505e
Signed-off-by: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/29404
Reviewed-by: QABUILD
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Tested-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33518
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

12 files changed, 421 insertions, 102 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index e4bf98d46ed..20a1d61690e 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -20,6 +20,8 @@
 #define ap_printk(...)
 #endif
 
+#define CW1200_LINK_ID_GC_TIMEOUT ((unsigned long)(10 * HZ))
+
 static int cw1200_upload_beacon(struct cw1200_common *priv);
 static int cw1200_start_ap(struct cw1200_common *priv);
 static int cw1200_update_beaconing(struct cw1200_common *priv);
@@ -35,27 +37,22 @@ 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 wsm_map_link map_link = {
-		.link_id = 0,
-	};
 
 	if (priv->mode != NL80211_IFTYPE_AP)
 		return 0;
 
-	map_link.link_id = ffs(~(priv->link_id_map | 1)) - 1;
-	if (map_link.link_id > CW1200_MAX_STA_IN_AP_MODE) {
-		sta_priv->link_id = 0;
-		printk(KERN_INFO "[AP] No more link ID available.\n");
+	sta_priv->link_id = cw1200_find_link_id(priv, sta->addr);
+	if (!sta_priv->link_id) {
+		wiphy_info(priv->hw->wiphy,
+			"[AP] No more link IDs available.\n");
 		return -ENOENT;
 	}
 
-	memcpy(map_link.mac_addr, sta->addr, ETH_ALEN);
-	if (!WARN_ON(wsm_map_link(priv, &map_link))) {
-		sta_priv->link_id = map_link.link_id;
-		priv->link_id_map |= BIT(map_link.link_id);
-		ap_printk(KERN_DEBUG "[AP] STA added, link_id: %d\n",
-			map_link.link_id);
-	}
+	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);
 	return 0;
 }
 
@@ -65,21 +62,19 @@ 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 wsm_reset reset = {
-		.link_id = 0,
-		.reset_statistics = false,
-	};
 
-	if (sta_priv->link_id) {
-		ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
-			sta_priv->link_id);
-		reset.link_id = sta_priv->link_id;
-		priv->link_id_map &= ~BIT(sta_priv->link_id);
-		sta_priv->link_id = 0;
-		wsm_lock_tx(priv);
-		WARN_ON(wsm_reset(priv, &reset));
-		wsm_unlock_tx(priv);
-	}
+	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;
+	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);
 	return 0;
 }
 
@@ -113,6 +108,40 @@ void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
 	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)
+{
+	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);
+	}
+}
+
+void cw1200_ps_notify(struct cw1200_common *priv,
+		      int link_id, bool ps)
+{
+	struct ieee80211_sta *sta;
+
+	if (!link_id || link_id > CW1200_MAX_STA_IN_AP_MODE)
+		return;
+
+	if (!!ps == !!priv->link_id_db[link_id - 1].ps)
+		return;
+
+	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);
+	rcu_read_unlock();
+}
+
 static int cw1200_set_tim_impl(struct cw1200_common *priv, bool aid0_bit_set)
 {
 	struct sk_buff *skb;
@@ -476,11 +505,14 @@ void cw1200_multicast_start_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, multicast_start_work);
+	long tmo = priv->join_dtim_period *
+			(priv->beacon_int + 20) * HZ / 1024;
 
 	if (!priv->aid0_bit_set) {
 		wsm_lock_tx(priv);
 		cw1200_set_tim_impl(priv, true);
 		priv->aid0_bit_set = true;
+		mod_timer(&priv->mcast_timeout, tmo);
 		wsm_unlock_tx(priv);
 	}
 }
@@ -498,6 +530,19 @@ void cw1200_multicast_stop_work(struct work_struct *work)
 	}
 }
 
+void cw1200_mcast_timeout(unsigned long arg)
+{
+	struct cw1200_common *priv =
+		(struct cw1200_common *)arg;
+
+	spin_lock_bh(&priv->buffered_multicasts_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);
+}
+
 int cw1200_ampdu_action(struct ieee80211_hw *hw,
 			struct ieee80211_vif *vif,
 			enum ieee80211_ampdu_mlme_action action,
@@ -522,6 +567,7 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 	u32 set = 0;
 	u32 clear;
 	u32 tx_suspend_mask;
+	bool cancel_tmo = false;
 	int i;
 
 	if (!arg->link_id) /* For all links */
@@ -542,16 +588,20 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 			/* Firmware sends this indication every DTIM if there
 			 * is a STA in powersave connected. There is no reason
 			 * to suspend, following wakeup will consume much more
-			 * power than could be saved. */
+			 * power than it could be saved. */
 			cw1200_pm_stay_awake(&priv->pm_state,
 					priv->join_dtim_period *
 					(priv->beacon_int + 20) * HZ / 1024);
 			priv->tx_multicast = priv->aid0_bit_set &&
 					priv->buffered_multicasts;
-			if (priv->tx_multicast)
+			if (priv->tx_multicast) {
+				cancel_tmo = true;
 				cw1200_bh_wakeup(priv);
+			}
 		}
 		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		if (cancel_tmo)
+			del_timer_sync(&priv->mcast_timeout);
 	} else {
 		if (arg->stop)
 			set = unicast;
@@ -563,6 +613,8 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 		/* 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;
@@ -576,6 +628,8 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 					&priv->tx_queue[i],
 					tx_suspend_mask & clear);
 		}
+		spin_unlock_bh(&priv->buffered_multicasts_lock);
+		cw1200_ps_notify(priv, arg->link_id, arg->stop);
 	}
 	if (wakeup_required)
 		cw1200_bh_wakeup(priv);
@@ -663,6 +717,8 @@ static int cw1200_start_ap(struct cw1200_common *priv)
 
 	memcpy(&start.ssid[0], priv->ssid, start.ssidLength);
 
+	memset(&priv->link_id_db, 0, sizeof(priv->link_id_db));
+
 	ap_printk(KERN_DEBUG "[AP] ch: %d(%d), bcn: %d(%d), brt: 0x%.8X, ssid: %.*s.\n",
 		start.channelNumber, start.band,
 		start.beaconInterval, start.DTIMPeriod,
@@ -706,3 +762,145 @@ static int cw1200_update_beaconing(struct cw1200_common *priv)
 	return 0;
 }
 
+int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	spin_lock_bh(&priv->buffered_multicasts_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) {
+			priv->link_id_db[i].timestamp = jiffies;
+			ret = i + 1;
+			break;
+		}
+	}
+	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	return ret;
+}
+
+int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	unsigned long max_inactivity = 0;
+	unsigned long now = jiffies;
+
+	spin_lock_bh(&priv->buffered_multicasts_lock);
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		if (!priv->link_id_db[i].status) {
+			ret = i + 1;
+			break;
+		} else if (priv->link_id_db[i].status != CW1200_LINK_HARD &&
+			!priv->tx_queue_stats.link_map_cache[i + 1]) {
+
+			unsigned long inactivity =
+					now - priv->link_id_db[i].timestamp;
+			if (inactivity < max_inactivity)
+				continue;
+			max_inactivity = inactivity;
+			ret = i + 1;
+		}
+	}
+	if (ret) {
+		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);
+		wsm_lock_tx_async(priv);
+		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
+			wsm_unlock_tx(priv);
+	} else {
+		wiphy_info(priv->hw->wiphy,
+			"[AP] Early: no more link IDs available.\n");
+	}
+
+	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	return ret;
+}
+
+void cw1200_link_id_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, link_id_work);
+	wsm_flush_tx(priv);
+	cw1200_link_id_gc_work(&priv->link_id_gc_work.work);
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_link_id_gc_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, link_id_gc_work.work);
+	struct wsm_reset reset = {
+		.reset_statistics = false,
+	};
+	struct wsm_map_link map_link = {
+		.link_id = 0,
+	};
+	unsigned long now = jiffies;
+	unsigned long next_gc = -1;
+	long ttl;
+	bool need_reset;
+	u32 mask;
+	int i, j;
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		return;
+
+	wsm_lock_tx(priv);
+	spin_lock_bh(&priv->buffered_multicasts_lock);
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		need_reset = false;
+		mask = BIT(i + 1);
+		if (priv->link_id_db[i].status == CW1200_LINK_RESERVE ||
+			(priv->link_id_db[i].status == CW1200_LINK_HARD &&
+			 !(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;
+				need_reset = true;
+			}
+			priv->link_id_map |= mask;
+			if (priv->link_id_db[i].status != CW1200_LINK_HARD)
+				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);
+			if (need_reset) {
+				reset.link_id = i + 1;
+				WARN_ON(wsm_reset(priv, &reset));
+			}
+			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);
+		} else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
+			ttl = priv->link_id_db[i].timestamp - now +
+					CW1200_LINK_ID_GC_TIMEOUT;
+			if (ttl <= 0) {
+				need_reset = true;
+				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;
+				memset(map_link.mac_addr, 0, ETH_ALEN);
+				spin_unlock_bh(&priv->buffered_multicasts_lock);
+				reset.link_id = i + 1;
+				WARN_ON(wsm_reset(priv, &reset));
+				spin_lock_bh(&priv->buffered_multicasts_lock);
+			} else {
+				next_gc = min(next_gc, (unsigned long)ttl);
+			}
+		}
+		if (need_reset)
+			ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
+					reset.link_id);
+	}
+	spin_unlock_bh(&priv->buffered_multicasts_lock);
+	if (next_gc != -1)
+		queue_delayed_work(priv->workqueue,
+				&priv->link_id_gc_work, next_gc);
+	wsm_unlock_tx(priv);
+}
+
diff --git a/drivers/staging/cw1200/ap.h b/drivers/staging/cw1200/ap.h
index 59462248bf4..032f631a656 100644
--- a/drivers/staging/cw1200/ap.h
+++ b/drivers/staging/cw1200/ap.h
@@ -36,6 +36,12 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 void cw1200_set_tim_work(struct work_struct *work);
 void cw1200_multicast_start_work(struct work_struct *work);
 void cw1200_multicast_stop_work(struct work_struct *work);
-
+void cw1200_mcast_timeout(unsigned long arg);
+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 e4fda896b78..85d7caf89e9 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -46,6 +46,7 @@
 
 #define CW1200_MAX_STA_IN_AP_MODE	(5)
 #define CW1200_LINK_ID_AFTER_DTIM	(CW1200_MAX_STA_IN_AP_MODE + 1)
+#define CW1200_MAX_REQUEUE_ATTEMPTS	(5)
 
 /* Please keep order */
 enum cw1200_join_status {
@@ -55,6 +56,20 @@ enum cw1200_join_status {
 	CW1200_JOIN_STATUS_AP,
 };
 
+enum cw1200_link_status {
+	CW1200_LINK_OFF,
+	CW1200_LINK_RESERVE,
+	CW1200_LINK_SOFT,
+	CW1200_LINK_HARD,
+};
+
+struct cw1200_link_entry {
+	unsigned long			timestamp;
+	enum cw1200_link_status		status;
+	u8				mac[ETH_ALEN];
+	bool				ps;
+};
+
 struct cw1200_common {
 	struct cw1200_queue		tx_queue[4];
 	struct cw1200_queue_stats	tx_queue_stats;
@@ -172,6 +187,9 @@ struct cw1200_common {
 
 	/* AP powersave */
 	u32			link_id_map;
+	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;
@@ -182,6 +200,7 @@ struct cw1200_common {
 	struct work_struct	set_tim_work;
 	struct work_struct	multicast_start_work;
 	struct work_struct	multicast_stop_work;
+	struct timer_list	mcast_timeout;
 
 
 	/* WSM events and CQM implementation */
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index c918d3f8291..2d7adfe8b4c 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -38,6 +38,12 @@ static const char * const cw1200_debug_fw_types[] = {
 	"Platform test",
 };
 
+static const char * const cw1200_debug_link_id[] = {
+	"OFF",
+	"REQ",
+	"SOFT",
+	"HARD",
+};
 
 static const char *cw1200_debug_mode(int mode)
 {
@@ -222,6 +228,17 @@ static int cw1200_status_show(struct seq_file *seq, void *v)
 
 	seq_puts(seq, "\n");
 
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		if (priv->link_id_db[i].status) {
+			seq_printf(seq, "Link %d:     %s, %pM\n",
+				i + 1, cw1200_debug_link_id[
+				priv->link_id_db[i].status],
+				priv->link_id_db[i].mac);
+		}
+	}
+
+	seq_puts(seq, "\n");
+
 	seq_printf(seq, "BH status:  %s\n",
 		atomic_read(&priv->bh_term) ? "terminated" : "alive");
 	seq_printf(seq, "Pending RX: %d\n",
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index a75fa6d8c50..57f90e9168a 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -251,6 +251,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	hw->flags = IEEE80211_HW_SIGNAL_DBM |
 		    IEEE80211_HW_SUPPORTS_PS |
 		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
+		    IEEE80211_HW_AP_LINK_PS |
 		    /* TODO: Fix this
 		     Disable UAPSD support due to performance drop */
 		    /* IEEE80211_HW_SUPPORTS_UAPSD | */
@@ -285,7 +286,9 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 
 	hw->max_rates = 8;
 	hw->max_rate_tries = 15;
-	hw->extra_tx_headroom = WSM_TX_EXTRA_HEADROOM;
+	hw->extra_tx_headroom = WSM_TX_EXTRA_HEADROOM +
+		8  /* TKIP IV */ +
+		12 /* TKIP ICV and MIC */;
 
 	hw->sta_data_size = sizeof(struct cw1200_sta_priv);
 
@@ -328,6 +331,11 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	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);
+	INIT_WORK(&priv->link_id_work, cw1200_link_id_work);
+	INIT_DELAYED_WORK(&priv->link_id_gc_work, cw1200_link_id_gc_work);
+	init_timer(&priv->mcast_timeout);
+	priv->mcast_timeout.data = (unsigned long)priv;
+	priv->mcast_timeout.function = cw1200_mcast_timeout;
 
 	if (unlikely(cw1200_pm_init(&priv->pm_state, priv))) {
 		ieee80211_free_hw(hw);
diff --git a/drivers/staging/cw1200/pm.c b/drivers/staging/cw1200/pm.c
index 48e46e6c3f3..3b6c0bf496c 100644
--- a/drivers/staging/cw1200/pm.c
+++ b/drivers/staging/cw1200/pm.c
@@ -26,6 +26,7 @@ struct cw1200_suspend_state {
 	unsigned long connection_loss_tmo;
 	unsigned long join_tmo;
 	unsigned long direct_probe;
+	unsigned long link_id_gc;
 };
 
 static struct dev_pm_ops cw1200_pm_ops = {
@@ -241,11 +242,17 @@ int cw1200_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 		cw1200_suspend_work(&priv->join_timeout);
 	state->direct_probe =
 		cw1200_suspend_work(&priv->scan.probe_work);
+	state->link_id_gc =
+		cw1200_suspend_work(&priv->link_id_gc_work);
 
 	/* Stop serving thread */
 	if (cw1200_bh_suspend(priv))
 		goto revert4;
 
+	ret = timer_pending(&priv->mcast_timeout);
+	if (ret)
+		goto revert5;
+
 	/* Store suspend state */
 	pm_state->suspend_state = state;
 
@@ -267,6 +274,8 @@ int cw1200_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 
 	return 0;
 
+revert5:
+	WARN_ON(cw1200_bh_resume(priv));
 revert4:
 	cw1200_resume_work(priv, &priv->bss_loss_work,
 			state->bss_loss_tmo);
@@ -276,6 +285,8 @@ revert4:
 			state->join_tmo);
 	cw1200_resume_work(priv, &priv->scan.probe_work,
 			state->direct_probe);
+	cw1200_resume_work(priv, &priv->link_id_gc_work,
+			state->link_id_gc);
 	kfree(state);
 revert3:
 	wsm_unlock_tx(priv);
@@ -309,6 +320,8 @@ int cw1200_wow_resume(struct ieee80211_hw *hw)
 			state->join_tmo);
 	cw1200_resume_work(priv, &priv->scan.probe_work,
 			state->direct_probe);
+	cw1200_resume_work(priv, &priv->link_id_gc_work,
+			state->link_id_gc);
 
 	/* Unlock datapath */
 	wsm_unlock_tx(priv);
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index a81442f8f4f..c6eabd12c56 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -221,6 +221,7 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 
 		list_move_tail(&item->head, &queue->queue);
 		item->skb = skb;
+		item->generation = 0;
 		item->packetID = cw1200_queue_make_packet_id(
 			queue->generation, queue->queue_id,
 			item->generation, item - queue->pool);
@@ -249,7 +250,8 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 int cw1200_queue_get(struct cw1200_queue *queue,
 		     u32 link_id_map,
 		     struct wsm_tx **tx,
-		     struct ieee80211_tx_info **tx_info)
+		     struct ieee80211_tx_info **tx_info,
+		     int *link_id)
 {
 	int ret = -ENOENT;
 	struct cw1200_queue_item *item;
@@ -267,6 +269,7 @@ int cw1200_queue_get(struct cw1200_queue *queue,
 	if (!WARN_ON(ret)) {
 		*tx = (struct wsm_tx *)item->skb->data;
 		*tx_info = IEEE80211_SKB_CB(item->skb);
+		*link_id = item->link_id;
 		list_move_tail(&item->head, &queue->pending);
 		++queue->num_pending;
 		--queue->link_map_cache[item->link_id];
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index fc5d4613efb..b28d0294d25 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -64,7 +64,8 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *cw1200,
 int cw1200_queue_get(struct cw1200_queue *queue,
 		     u32 link_id_map,
 		     struct wsm_tx **tx,
-		     struct ieee80211_tx_info **tx_info);
+		     struct ieee80211_tx_info **tx_info,
+		     int *link_id);
 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID);
 int cw1200_queue_requeue_all(struct cw1200_queue *queue);
 int cw1200_queue_remove(struct cw1200_queue *queue, struct cw1200_common *priv,
@@ -79,12 +80,14 @@ void cw1200_queue_unlock(struct cw1200_queue *queue,
 bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
 				 u32 link_id_map);
 
-/* int cw1200_queue_get_stats(struct cw1200_queue *queue,
-struct ieee80211_tx_queue_stats *stats); */
-
 static inline u8 cw1200_queue_get_queue_id(u32 packetID)
 {
 	return (packetID >> 16) & 0xFF;
 }
 
+static inline u8 cw1200_queue_get_generation(u32 packetID)
+{
+	return (packetID >>  8) & 0xFF;
+}
+
 #endif /* CW1200_QUEUE_H_INCLUDED */
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index b35dbca7ba0..f6227833248 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -125,6 +125,7 @@ void cw1200_stop(struct ieee80211_hw *dev)
 	cancel_delayed_work_sync(&priv->join_timeout);
 	cancel_delayed_work_sync(&priv->bss_loss_work);
 	cancel_delayed_work_sync(&priv->connection_loss_work);
+	cancel_delayed_work_sync(&priv->link_id_gc_work);
 
 	mutex_lock(&priv->conf_mutex);
 	switch (priv->join_status) {
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 18544011b16..1742af0ac8e 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -10,6 +10,7 @@
  */
 
 #include <net/mac80211.h>
+#include <linux/etherdevice.h>
 
 #include "cw1200.h"
 #include "wsm.h"
@@ -420,17 +421,35 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_hdr *hdr =
 		(struct ieee80211_hdr *)skb->data;
+	const u8 *da = ieee80211_get_DA(hdr);
 	struct cw1200_sta_priv *sta_priv =
 		(struct cw1200_sta_priv *)&tx_info->control.sta->drv_priv;
-	int link_id = 0;
+	int link_id;
 	int ret;
+	int i;
 
-	if (tx_info->control.sta)
+	if (likely(tx_info->control.sta && sta_priv->link_id))
 		link_id = sta_priv->link_id;
-	else if ((tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
-			(priv->mode == NL80211_IFTYPE_AP) &&
-			priv->enable_beacon)
-		link_id = CW1200_LINK_ID_AFTER_DTIM;
+	else if (priv->mode != NL80211_IFTYPE_AP)
+		link_id = 0;
+	else if (is_multicast_ether_addr(da)) {
+		if (priv->enable_beacon)
+			link_id = CW1200_LINK_ID_AFTER_DTIM;
+		else
+			link_id = 0;
+	} else {
+		link_id = cw1200_find_link_id(priv, da);
+		if (!link_id)
+			link_id = cw1200_alloc_link_id(priv, da);
+		if (!link_id) {
+			wiphy_err(priv->hw->wiphy,
+				"%s: No more link IDs available.\n",
+				__func__);
+			goto err;
+		}
+	}
+	if (link_id && link_id <= CW1200_MAX_STA_IN_AP_MODE)
+		priv->link_id_db[link_id - 1].timestamp = jiffies;
 
 	txrx_printk(KERN_DEBUG "[TX] TX %d bytes (queue: %d, link_id: %d).\n",
 			skb->len, queue, link_id);
@@ -438,22 +457,13 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	if (WARN_ON(queue >= 4))
 		goto err;
 
-#if 0
-	{
-		/* HACK!!!
-		* Workarounnd against a bug in WSM_A21.05.0288 firmware.
-		* In AP mode FW calculates FCS incorrectly when DA
-		* is FF:FF:FF:FF:FF:FF. Just for verification,
-		* do not enable this code in the real live. */
-		static const u8 mac_ff[] =
-			{0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-		static const u8 mac_mc[] =
-			{0x01, 0x00, 0x5e, 0x00, 0x00, 0x16};
-		if (!memcmp(&skb->data[4], mac_ff, sizeof(mac_ff)))
-			memcpy(&skb->data[4], mac_mc, sizeof(mac_mc));
+	if (unlikely(ieee80211_is_auth(hdr->frame_control))) {
+		spin_lock_bh(&priv->buffered_multicasts_lock);
+		priv->sta_asleep_mask &= ~BIT(link_id);
+		for (i = 0; i < 4; ++i)
+			priv->tx_suspend_mask[i] &= ~BIT(link_id);
+		spin_unlock_bh(&priv->buffered_multicasts_lock);
 	}
-#endif
-
 
 	/* IV/ICV injection. */
 	/* TODO: Quite unoptimal. It's better co modify mac80211
@@ -471,8 +481,9 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 			icv_len += 8; /* MIC */
 		}
 
-		if (skb_headroom(skb) < iv_len + WSM_TX_EXTRA_HEADROOM
-				|| skb_tailroom(skb) < icv_len) {
+		if ((skb_headroom(skb) + skb_tailroom(skb) <
+			iv_len + icv_len + WSM_TX_EXTRA_HEADROOM) ||
+			(skb_headroom(skb) < iv_len + WSM_TX_EXTRA_HEADROOM)) {
 			wiphy_err(priv->hw->wiphy,
 				"Bug: no space allocated "
 				"for crypto headers.\n"
@@ -482,6 +493,17 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 				skb_headroom(skb), skb_tailroom(skb),
 				iv_len + WSM_TX_EXTRA_HEADROOM, icv_len);
 			goto err;
+		} else if (skb_tailroom(skb) < icv_len) {
+			size_t offset = icv_len - skb_tailroom(skb);
+			u8 *p;
+			wiphy_warn(priv->hw->wiphy,
+				"Slowpath: tailroom is not big enough. "
+				"Req: %d, got: %d.\n",
+				icv_len, skb_tailroom(skb));
+
+			p = skb_push(skb, offset);
+			memmove(p, &p[offset], skb->len - offset);
+			skb_trim(skb, skb->len - offset);
 		}
 
 		newhdr = skb_push(skb, iv_len);
@@ -572,9 +594,9 @@ static int cw1200_handle_pspoll(struct cw1200_common *priv,
 				struct sk_buff *skb)
 {
 	struct ieee80211_sta *sta;
-	struct cw1200_sta_priv *sta_priv;
 	struct ieee80211_pspoll *pspoll =
 		(struct ieee80211_pspoll *) skb->data;
+	int link_id = 0;
 	u32 pspoll_mask;
 	int drop = 1;
 	int i;
@@ -586,13 +608,21 @@ static int cw1200_handle_pspoll(struct cw1200_common *priv,
 
 	rcu_read_lock();
 	sta = ieee80211_find_sta(priv->vif, pspoll->ta);
-	if (!sta) {
-		rcu_read_unlock();
-		goto done;
+	if (sta) {
+		struct cw1200_sta_priv *sta_priv;
+		sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
+		link_id = sta_priv->link_id;
+		pspoll_mask = BIT(sta_priv->link_id);
 	}
-	sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
-	pspoll_mask = BIT(sta_priv->link_id);
 	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;
@@ -608,6 +638,7 @@ static int cw1200_handle_pspoll(struct cw1200_common *priv,
 			break;
 		}
 	}
+	txrx_printk(KERN_DEBUG "[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
 done:
 	return drop;
 }
@@ -631,6 +662,10 @@ void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 	if (WARN_ON(queue_id >= 4))
 		return;
 
+	if (arg->status)
+		txrx_printk(KERN_DEBUG "TX failed: %d.\n",
+				arg->status);
+
 	if ((arg->status == WSM_REQUEUE) &&
 	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
 		/* "Requeue" means "implicit suspend" */
@@ -640,20 +675,10 @@ void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 			.multicast = !arg->link_id,
 		};
 		cw1200_suspend_resume(priv, &suspend);
-		if (suspend.multicast) {
-			/* HACK!!! WSM324 firmware has tendency to requeue
-			 * multicast frames in a loop, causing performance
-			 * drop and high power consumption of the driver.
-			 * In this situation it is better just to drop
-			 * the problematic frame. */
-			wiphy_warn(priv->hw->wiphy, "Attempt to requeue a "
-					"multicat frame. Frame is dropped\n");
-			WARN_ON(cw1200_queue_remove(queue, priv,
-					arg->packetID));
-		} else {
-			WARN_ON(cw1200_queue_requeue(queue,
-					arg->packetID));
-		}
+		wiphy_warn(priv->hw->wiphy, "Requeue (try %d).\n",
+			cw1200_queue_get_generation(arg->packetID) + 1);
+		WARN_ON(cw1200_queue_requeue(queue,
+				arg->packetID));
 	} else if (!WARN_ON(cw1200_queue_get_skb(
 			queue, arg->packetID, &skb))) {
 		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
@@ -719,8 +744,8 @@ 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;
 	unsigned long grace_period;
-	__le16 frame_control;
 	hdr->flag = 0;
 
 	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
@@ -728,6 +753,9 @@ 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 (unlikely(arg->status)) {
 		if (arg->status == WSM_STATUS_MICFAILURE) {
 			txrx_printk(KERN_DEBUG "[RX] MIC failure.\n");
@@ -748,9 +776,7 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 		goto drop;
 	}
 
-	frame_control = *(__le16*)skb->data;
-
-	if (unlikely(ieee80211_is_pspoll(frame_control)))
+	if (unlikely(ieee80211_is_pspoll(frame->frame_control)))
 		if (cw1200_handle_pspoll(priv, skb))
 			goto drop;
 
@@ -775,7 +801,7 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 
 	if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
 		size_t iv_len = 0, icv_len = 0;
-		size_t hdrlen = ieee80211_hdrlen(frame_control);
+		size_t hdrlen = ieee80211_hdrlen(frame->frame_control);
 
 		hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
 
@@ -821,7 +847,7 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 	if (arg->flags & WSM_RX_STATUS_AGGREGATE)
 		cw1200_debug_rxed_agg(priv);
 
-	if (ieee80211_is_action(frame_control) &&
+	if (ieee80211_is_action(frame->frame_control) &&
 			(arg->flags & WSM_RX_STATUS_ADDRESS1))
 		if (cw1200_handle_action_rx(priv, skb))
 			return;
@@ -829,12 +855,16 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 	/* Stay awake for 1sec. after frame is received to give
 	 * userspace chance to react and acquire appropriate
 	 * wakelock. */
-	if (ieee80211_is_auth(frame_control))
+	if (ieee80211_is_auth(frame->frame_control))
 		grace_period = 5 * HZ;
 	else
 		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. */
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 2ad3eceaf69..6514b9227a7 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -851,6 +851,7 @@ underflow:
 }
 
 static int wsm_receive_indication(struct cw1200_common *priv,
+					int link_id,
 					struct wsm_buf *buf,
 					struct sk_buff **skb_p)
 {
@@ -865,6 +866,7 @@ static int wsm_receive_indication(struct cw1200_common *priv,
 		rx.rxedRate = WSM_GET8(buf);
 		rx.rcpiRssi = WSM_GET8(buf);
 		rx.flags = WSM_GET32(buf);
+		rx.link_id = link_id;
 		fctl = *(__le16 *)buf->data;
 		hdr_len = buf->data - buf->begin;
 		skb_pull(*skb_p, hdr_len);
@@ -1276,7 +1278,8 @@ int wsm_handle_rx(struct cw1200_common *priv, int id,
 			ret = wsm_startup_indication(priv, &wsm_buf);
 			break;
 		case 0x0804:
-			ret = wsm_receive_indication(priv, &wsm_buf, skb_p);
+			ret = wsm_receive_indication(priv, link_id,
+					&wsm_buf, skb_p);
 			break;
 		case 0x0805:
 			ret = wsm_event_indication(priv, &wsm_buf);
@@ -1309,7 +1312,8 @@ out:
 
 static bool wsm_handle_tx_data(struct cw1200_common *priv,
 			       const struct wsm_tx *wsm,
-			       const struct ieee80211_tx_info *tx_info)
+			       const struct ieee80211_tx_info *tx_info,
+			       int *link_id)
 {
 	bool handled = false;
 	const struct ieee80211_hdr *frame =
@@ -1344,6 +1348,24 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	case NL80211_IFTYPE_AP:
 		if (unlikely(!priv->join_status))
 			action = doDrop;
+		if (*link_id == CW1200_LINK_ID_AFTER_DTIM)
+			*link_id = 0;
+		else if (WARN_ON(!(BIT(*link_id) &
+				(BIT(0) | priv->link_id_map))))
+			action = doDrop;
+		if (cw1200_queue_get_generation(wsm->packetID) >
+				CW1200_MAX_REQUEUE_ATTEMPTS) {
+			/* HACK!!! WSM324 firmware has tendency to requeue
+			 * multicast frames in a loop, causing performance
+			 * drop and high power consumption of the driver.
+			 * In this situation it is better just to drop
+			 * the problematic frame. */
+			wiphy_warn(priv->hw->wiphy,
+					"Too many attempts "
+					"to requeue a frame. "
+					"Frame is dropped.\n");
+			action = doDrop;
+		}
 		break;
 	case NL80211_IFTYPE_ADHOC:
 	case NL80211_IFTYPE_MESH_POINT:
@@ -1550,8 +1572,8 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 	struct wsm_tx *wsm = NULL;
 	struct ieee80211_tx_info *tx_info;
 	struct cw1200_queue *queue;
-	struct cw1200_sta_priv *sta_priv;
 	u32 tx_allowed_mask = 0;
+	int link_id;
 	/*
 	 * Count was intended as an input for wsm->more flag.
 	 * During implementation it was found that wsm->more
@@ -1600,26 +1622,22 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 
 			if (cw1200_queue_get(queue,
 					tx_allowed_mask,
-					&wsm, &tx_info))
+					&wsm, &tx_info, &link_id))
 				continue;
 
-			if (wsm_handle_tx_data(priv, wsm, tx_info))
+			if (wsm_handle_tx_data(priv, wsm, tx_info, &link_id))
 				continue;  /* Handled by WSM */
 
-			if (tx_info->control.sta) {
-				/* Update link id */
-				sta_priv = (struct cw1200_sta_priv *)
-					&tx_info->control.sta->drv_priv;
-				wsm->hdr.id &= __cpu_to_le16(
-					~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX));
-				wsm->hdr.id |= cpu_to_le16(
-					WSM_TX_LINK_ID(sta_priv->link_id));
-				priv->pspoll_mask &= ~BIT(sta_priv->link_id);
-			}
+			wsm->hdr.id &= __cpu_to_le16(
+				~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX));
+			wsm->hdr.id |= cpu_to_le16(
+				WSM_TX_LINK_ID(link_id));
+			priv->pspoll_mask &= ~BIT(link_id);
 
 			*data = (u8 *)wsm;
 			*tx_len = __le16_to_cpu(wsm->hdr.len);
 
+
 			if (more) {
 				struct ieee80211_hdr *hdr =
 					(struct ieee80211_hdr *) &wsm[1];
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 6b6d4a8049e..e9f7c922994 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -807,6 +807,9 @@ struct wsm_rx {
 
 	/* Size of the frame */
 	/* [out] */ size_t frame_size;
+
+	/* Link ID */
+	/* [out] */ int link_id;
 };
 
 /* = sizeof(generic hi hdr) + sizeof(wsm hdr) */