cw1200: Requeue special frames.

Some frames require special handling in wsm, for example
offchannel, wep, join...
Requeue for offchannel and wep frames was missing, leading
to queue entry leakage and unexpected drop of frames.

Fix implements requeue for special frames.

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

6 files changed, 54 insertions, 30 deletions

diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 239c3a9f3b9..3c3a50ce318 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -178,7 +178,7 @@ struct cw1200_common {
 	/* WSM Join */
 	enum cw1200_join_status	join_status;
 	u8			join_bssid[ETH_ALEN];
-	const struct wsm_tx	*join_pending_frame;
+	u32			pending_frame_id;
 	struct work_struct	join_work;
 	struct delayed_work	join_timeout;
 	struct work_struct	unjoin_work;
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index 12152d33684..b1069255790 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -494,7 +494,8 @@ int cw1200_queue_remove(struct cw1200_queue *queue, u32 packetID)
 }
 
 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
-			 struct sk_buff **skb)
+			 struct sk_buff **skb,
+			 const struct cw1200_txpriv **txpriv)
 {
 	int ret = 0;
 	u8 queue_generation, queue_id, item_generation, item_id;
@@ -516,6 +517,7 @@ int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
 		ret = -ENOENT;
 	} else {
 		*skb = item->skb;
+		*txpriv = &item->txpriv;
 	}
 	spin_unlock_bh(&queue->lock);
 	return ret;
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index bff33625c8a..2403b2519ba 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -92,7 +92,8 @@ int cw1200_queue_requeue_all(struct cw1200_queue *queue);
 int cw1200_queue_remove(struct cw1200_queue *queue,
 			u32 packetID);
 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
-			 struct sk_buff **skb);
+			 struct sk_buff **skb,
+			 const struct cw1200_txpriv **txpriv);
 void cw1200_queue_lock(struct cw1200_queue *queue);
 void cw1200_queue_unlock(struct cw1200_queue *queue);
 
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index 2ddbc1aa2b0..908294c8a93 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -725,14 +725,19 @@ void cw1200_wep_key_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, wep_key_work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
 	__le32 wep_default_key_id = __cpu_to_le32(
 		priv->wep_default_key_id);
 
+	BUG_ON(queueId >= 4);
+
 	sta_printk(KERN_DEBUG "[STA] Setting default WEP key: %d\n",
 		priv->wep_default_key_id);
 	wsm_flush_tx(priv);
 	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
 		&wep_default_key_id, sizeof(wep_default_key_id)));
+	cw1200_queue_requeue(queue, priv->pending_frame_id);
 	wsm_unlock_tx(priv);
 }
 
@@ -1096,15 +1101,22 @@ void cw1200_offchannel_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, offchannel_work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
 
+	BUG_ON(queueId >= 4);
 	BUG_ON(!priv->channel);
 
 	mutex_lock(&priv->conf_mutex);
-	if (!priv->join_status) {
+	if (likely(!priv->join_status)) {
 		wsm_flush_tx(priv);
 		cw1200_update_listening(priv, true);
 		cw1200_update_filtering(priv);
 	}
+	if (unlikely(!priv->join_status))
+		cw1200_queue_remove(queue, priv->pending_frame_id);
+	else
+		cw1200_queue_requeue(queue, priv->pending_frame_id);
 	mutex_unlock(&priv->conf_mutex);
 	wsm_unlock_tx(priv);
 }
@@ -1113,20 +1125,31 @@ void cw1200_join_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, join_work);
-	const struct wsm_tx *wsm = priv->join_pending_frame;
-	const struct ieee80211_hdr *frame = (struct ieee80211_hdr *)&wsm[1];
-	const u8 *bssid = &frame->addr1[0]; /* AP SSID in a 802.11 frame */
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
+	const struct cw1200_txpriv *txpriv = NULL;
+	struct sk_buff *skb = NULL;
+	const struct wsm_tx *wsm;
+	const struct ieee80211_hdr *frame;
+	const u8 *bssid;
 	struct cfg80211_bss *bss;
 	const u8 *ssidie;
 	const u8 *dtimie;
 	const struct ieee80211_tim_ie *tim = NULL;
-	u8 queueId;
+
+	BUG_ON(queueId >= 4);
+	if (cw1200_queue_get_skb(queue,	priv->pending_frame_id,
+			&skb, &txpriv)) {
+		wsm_unlock_tx(priv);
+		return;
+	}
+	wsm = (struct wsm_tx *)&skb->data[0];
+	frame = (struct ieee80211_hdr *)&skb->data[txpriv->offset];
+	bssid = &frame->addr1[0]; /* AP SSID in a 802.11 frame */
 
 	BUG_ON(!wsm);
 	BUG_ON(!priv->channel);
 
-	queueId = wsm_queue_id_to_linux(wsm->queueId);
-
 	if (unlikely(priv->join_status)) {
 		wsm_lock_tx(priv);
 		cw1200_unjoin_work(&priv->unjoin_work);
@@ -1134,12 +1157,10 @@ void cw1200_join_work(struct work_struct *work)
 
 	cancel_delayed_work_sync(&priv->join_timeout);
 
-	priv->join_pending_frame = NULL;
 	bss = cfg80211_get_bss(priv->hw->wiphy, priv->channel,
 			bssid, NULL, 0, 0, 0);
 	if (!bss) {
-		cw1200_queue_remove(&priv->tx_queue[queueId],
-			__le32_to_cpu(wsm->packetID));
+		cw1200_queue_remove(queue, priv->pending_frame_id);
 		wsm_unlock_tx(priv);
 		return;
 	}
@@ -1213,16 +1234,14 @@ void cw1200_join_work(struct work_struct *work)
 		if (wsm_join(priv, &join)) {
 			memset(&priv->join_bssid[0],
 				0, sizeof(priv->join_bssid));
-			cw1200_queue_remove(&priv->tx_queue[queueId],
-				__le32_to_cpu(wsm->packetID));
+			cw1200_queue_remove(queue, priv->pending_frame_id);
 			cancel_delayed_work_sync(&priv->join_timeout);
 			cw1200_update_listening(priv, priv->listening);
 		} else {
 			/* Upload keys */
 			WARN_ON(cw1200_upload_keys(priv));
 			WARN_ON(wsm_keep_alive_period(priv, 30 /* sec */));
-			cw1200_queue_requeue(&priv->tx_queue[queueId],
-				__le32_to_cpu(wsm->packetID));
+			cw1200_queue_requeue(queue, priv->pending_frame_id);
 			priv->join_status = CW1200_JOIN_STATUS_STA;
 		}
 		WARN_ON(wsm_set_block_ack_policy(priv,
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index d49f5dce09e..ef5accb19a0 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -828,6 +828,7 @@ void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 	u8 queue_id = cw1200_queue_get_queue_id(arg->packetID);
 	struct cw1200_queue *queue = &priv->tx_queue[queue_id];
 	struct sk_buff *skb;
+	const struct cw1200_txpriv *txpriv;
 
 	txrx_printk(KERN_DEBUG "[TX] TX confirm: %d, %d.\n",
 		arg->status, arg->ackFailures);
@@ -861,7 +862,7 @@ void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 		WARN_ON(cw1200_queue_requeue(queue,
 				arg->packetID));
 	} else if (!WARN_ON(cw1200_queue_get_skb(
-			queue, arg->packetID, &skb))) {
+			queue, arg->packetID, &skb, &txpriv))) {
 		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
 		int tx_count = arg->ackFailures;
 		u8 ht_flags = 0;
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 19f87bed526..10303fae3c8 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1313,6 +1313,7 @@ out:
 static bool wsm_handle_tx_data(struct cw1200_common *priv,
 			       const struct wsm_tx *wsm,
 			       const struct ieee80211_tx_info *tx_info,
+			       struct cw1200_queue *queue,
 			       int link_id)
 {
 	bool handled = false;
@@ -1395,24 +1396,24 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	switch (action) {
 	case doProbe:
 	{
+		const struct cw1200_txpriv *txpriv;
 		/* An interesting FW "feature". Device filters
 		 * probe responses.
 		 * The easiest way to get it back is to convert
 		 * probe request into WSM start_scan command. */
-		struct cw1200_queue *queue =
-			&priv->tx_queue[cw1200_queue_get_queue_id(
-				wsm->packetID)];
 		wsm_printk(KERN_DEBUG \
 			"[WSM] Convert probe request to scan.\n");
 		wsm_lock_tx_async(priv);
 		BUG_ON(priv->scan.probe_skb);
 		BUG_ON(cw1200_queue_get_skb(queue,
 				wsm->packetID,
-				&priv->scan.probe_skb));
+				&priv->scan.probe_skb,
+				&txpriv));
 		skb_get(priv->scan.probe_skb);
 		IEEE80211_SKB_CB(priv->scan.probe_skb)->flags |=
 				IEEE80211_TX_STAT_ACK;
-		BUG_ON(cw1200_queue_remove(queue, wsm->packetID));
+		BUG_ON(cw1200_queue_remove(queue,
+			__le32_to_cpu(wsm->packetID)));
 		/* Release used TX rate policy */
 		queue_delayed_work(priv->workqueue,
 				&priv->scan.probe_work, 0);
@@ -1423,11 +1424,9 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	{
 		/* See detailed description of "join" below.
 		 * We are dropping everything except AUTH in non-joined mode. */
-		struct cw1200_queue *queue =
-			&priv->tx_queue[cw1200_queue_get_queue_id(
-				wsm->packetID)];
 		wsm_printk(KERN_DEBUG "[WSM] Drop frame (0x%.4X).\n", fctl);
-		BUG_ON(cw1200_queue_remove(queue, wsm->packetID));
+		BUG_ON(cw1200_queue_remove(queue,
+			__le32_to_cpu(wsm->packetID)));
 		handled = true;
 	}
 	break;
@@ -1441,8 +1440,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 		 * not require protection */
 		wsm_printk(KERN_DEBUG "[WSM] Issue join command.\n");
 		wsm_lock_tx_async(priv);
-		BUG_ON(priv->join_pending_frame);
-		priv->join_pending_frame = wsm;
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
 		if (queue_work(priv->workqueue, &priv->join_work) <= 0)
 			wsm_unlock_tx(priv);
 		handled = true;
@@ -1452,6 +1450,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	{
 		wsm_printk(KERN_DEBUG "[WSM] Offchannel TX request.\n");
 		wsm_lock_tx_async(priv);
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
 		if (queue_work(priv->workqueue, &priv->offchannel_work) <= 0)
 			wsm_unlock_tx(priv);
 		handled = true;
@@ -1462,6 +1461,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 		wsm_printk(KERN_DEBUG "[WSM] Issue set_default_wep_key.\n");
 		wsm_lock_tx_async(priv);
 		priv->wep_default_key_id = tx_info->control.hw_key->keyidx;
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
 		if (queue_work(priv->workqueue, &priv->wep_key_work) <= 0)
 			wsm_unlock_tx(priv);
 		handled = true;
@@ -1619,7 +1619,8 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 				continue;
 
 			if (wsm_handle_tx_data(priv, wsm,
-					tx_info, txpriv->raw_link_id))
+					tx_info, queue,
+					txpriv->raw_link_id))
 				continue;  /* Handled by WSM */
 
 			wsm->hdr.id &= __cpu_to_le16(