cw1200: Fix for firmware exception on requeued TXed frames.

TX sequence number was not cleared properly in requeued
frames. Firmware checked it and triggered exception.

Fix clears TX sequence part of id field before applying (or-ing)
a new sequence number.

Also handling of TX confirmation frames with "requeue" status was
slightly changed with respect to PS flow control (suspend/resume link).
Note that multicast TX-ing in SoftAP mode still be completly broken,
to be fixed later.

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

5 files changed, 49 insertions, 22 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index b9e91fb07b7..edcd4af885a 100644
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -398,7 +398,6 @@ void cw1200_multicast_start_work(struct work_struct *work)
 		container_of(work, struct cw1200_common, multicast_start_work);
 
         (void)cw1200_set_tim_impl(priv, true);
-	wsm_lock_tx(priv);
 	priv->suspend_multicast = false;
 	wsm_unlock_tx(priv);
 	cw1200_bh_wakeup(priv);
@@ -409,9 +408,8 @@ void cw1200_multicast_stop_work(struct work_struct *work)
         struct cw1200_common *priv =
                 container_of(work, struct cw1200_common, multicast_stop_work);
 
-	/* Lock flushes send queue in device. Just to make sure DTIM beacom
-	 * and frames are sent. */
-	wsm_lock_tx(priv);
+	/* Flush to make sure DTIM beacon and frames are sent. */
+	wsm_flush_tx(priv);
 	priv->suspend_multicast = true;
         (void)cw1200_set_tim_impl(priv, false);
 	wsm_unlock_tx(priv);
@@ -452,15 +450,17 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 		arg->multicast ? "broadcast" : "unicast");
 
 	if (arg->multicast) {
-		if (arg->stop)
+		if (arg->stop) {
+			wsm_lock_tx_async(priv);
 			queue_work(priv->workqueue,
 				&priv->multicast_stop_work);
-		else {
+		} else {
 			/* Handle only if there is data to be sent */
 			for (i = 0; i < 4; ++i) {
 				if (cw1200_queue_get_num_queued(
 						&priv->tx_queue[i],
 						after_dtim)) {
+					wsm_lock_tx_async(priv);
 					queue_work(priv->workqueue,
 						&priv->multicast_start_work);
 					break;
diff --git a/drivers/staging/cw1200/bh.c b/drivers/staging/cw1200/bh.c
index 2d438a96e38..cddfd9dfaa4 100644
--- a/drivers/staging/cw1200/bh.c
+++ b/drivers/staging/cw1200/bh.c
@@ -476,8 +476,10 @@ tx:
 				}
 #endif /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
 
-				wsm->id |= __cpu_to_le32(
-					priv->wsm_tx_seq << 13);
+				wsm->id &= __cpu_to_le32(
+						~WSM_TX_SEQ(WSM_TX_SEQ_MAX));
+				wsm->id |= cpu_to_le32(
+						WSM_TX_SEQ(priv->wsm_tx_seq));
 
 				if (WARN_ON(cw1200_data_write(priv,
 				    data, tx_len))) {
@@ -491,7 +493,8 @@ tx:
 #endif /* CONFIG_CW1200_WSM_DUMPS */
 
 				wsm_txed(priv, data);
-				priv->wsm_tx_seq = (priv->wsm_tx_seq + 1) & 7;
+				priv->wsm_tx_seq = (priv->wsm_tx_seq + 1) &
+						WSM_TX_SEQ_MAX;
 			}
 		}
 
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 5db1282b188..7616c237a88 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -14,6 +14,7 @@
 #include "cw1200.h"
 #include "wsm.h"
 #include "bh.h"
+#include "ap.h"
 #include "debug.h"
 
 #if defined(CONFIG_CW1200_TX_POLICY_DEBUG)
@@ -575,8 +576,16 @@ void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 
 	if ((arg->status == WSM_REQUEUE) &&
 	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
+		/* "Requeue" means "implicit suspend" */
+		struct wsm_suspend_resume suspend = {
+			.link_id = arg->link_id,
+			.stop = 1,
+			.multicast = !arg->link_id,
+		};
+		cw1200_suspend_resume(priv, &suspend);
 		WARN_ON(cw1200_queue_requeue(queue, arg->packetID));
-	} else if (!WARN_ON(cw1200_queue_get_skb(queue, arg->packetID, &skb))) {
+	} else if (!WARN_ON(cw1200_queue_get_skb(
+			queue, arg->packetID, &skb))) {
 		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
 		struct wsm_tx *wsm_tx = (struct wsm_tx *)skb->data;
 		int rate_id = (wsm_tx->flags >> 4) & 0x07;
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 46280046bae..604fa067ccc 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -179,7 +179,7 @@ int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg)
 {
 	int ret;
 	struct wsm_buf *buf = &priv->wsm_cmd_buf;
-	u16 cmd = 0x000A | ((arg->link_id & 0x0F) << 6);
+	u16 cmd = 0x000A | WSM_TX_LINK_ID(arg->link_id);
 
 	wsm_cmd_lock(priv);
 
@@ -361,7 +361,9 @@ int wsm_stop_scan(struct cw1200_common *priv)
 }
 
 
-static int wsm_tx_confirm(struct cw1200_common *priv, struct wsm_buf *buf)
+static int wsm_tx_confirm(struct cw1200_common *priv,
+			  struct wsm_buf *buf,
+			  int link_id)
 {
 	struct wsm_tx_confirm tx_confirm;
 
@@ -372,6 +374,7 @@ static int wsm_tx_confirm(struct cw1200_common *priv, struct wsm_buf *buf)
 	tx_confirm.flags = WSM_GET16(buf);
 	tx_confirm.mediaDelay = WSM_GET32(buf);
 	tx_confirm.txQueueDelay = WSM_GET32(buf);
+	tx_confirm.link_id = link_id;
 
 	if (priv->wsm_cbc.tx_confirm)
 		priv->wsm_cbc.tx_confirm(priv, &tx_confirm);
@@ -383,7 +386,7 @@ underflow:
 }
 
 static int wsm_multi_tx_confirm(struct cw1200_common *priv,
-				struct wsm_buf *buf)
+				struct wsm_buf *buf, int link_id)
 {
 	int ret;
 	int count;
@@ -402,7 +405,7 @@ static int wsm_multi_tx_confirm(struct cw1200_common *priv,
 
 	cw1200_debug_txed_multi(priv, count);
 	for (i = 0; i < count; ++i) {
-		ret = wsm_tx_confirm(priv, buf);
+		ret = wsm_tx_confirm(priv, buf, link_id);
 		if (ret)
 			return ret;
 	}
@@ -754,7 +757,7 @@ int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg)
 {
 	int ret;
 	struct wsm_buf *buf = &priv->wsm_cmd_buf;
-	u16 cmd = 0x001C | ((arg->link_id & 0x0F) << 6);
+	u16 cmd = 0x001C | WSM_TX_LINK_ID(arg->link_id);
 
 	wsm_cmd_lock(priv);
 
@@ -1109,7 +1112,7 @@ int wsm_handle_rx(struct cw1200_common *priv, int id,
 	int link_id = (id >> 6) & 0x0F;
 
 	/* Strip link id. */
-	id &= ~(0x0F << 6);
+	id &= ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
 
 	wsm_buf.begin = (u8 *)&wsm[0];
 	wsm_buf.data = (u8 *)&wsm[1];
@@ -1119,9 +1122,9 @@ int wsm_handle_rx(struct cw1200_common *priv, int id,
 			wsm_buf.end - wsm_buf.begin);
 
 	if (id == 0x404) {
-		ret = wsm_tx_confirm(priv, &wsm_buf);
+		ret = wsm_tx_confirm(priv, &wsm_buf, link_id);
 	} else if (id == 0x41E) {
-		ret = wsm_multi_tx_confirm(priv, &wsm_buf);
+		ret = wsm_multi_tx_confirm(priv, &wsm_buf, link_id);
 	} else if (id & 0x0400) {
 		void *wsm_arg;
 		u16 wsm_cmd;
@@ -1130,7 +1133,8 @@ int wsm_handle_rx(struct cw1200_common *priv, int id,
 		 * response and race condition removal (see above). */
 		spin_lock(&priv->wsm_cmd.lock);
 		wsm_arg = priv->wsm_cmd.arg;
-		wsm_cmd = priv->wsm_cmd.cmd & ~(0x0F << 6);
+		wsm_cmd = priv->wsm_cmd.cmd &
+				~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
 		priv->wsm_cmd.cmd = 0xFFFF;
 		spin_unlock(&priv->wsm_cmd.lock);
 
@@ -1501,9 +1505,10 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 				/* Update link id */
 				sta_priv = (struct cw1200_sta_priv *)
 					&tx_info->control.sta->drv_priv;
-				wsm->hdr.id &= __cpu_to_le16(~(0x0F << 6));
-				wsm->hdr.id |=
-					cpu_to_le16(sta_priv->link_id << 6);
+				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));
 			}
 
 			*data = (u8 *)wsm;
@@ -1519,6 +1524,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 			} else if (priv->mode == NL80211_IFTYPE_AP &&
 					!priv->suspend_multicast) {
 				priv->suspend_multicast = true;
+				wsm_lock_tx_async(priv);
 				queue_work(priv->workqueue,
 					&priv->multicast_stop_work);
 			}
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 9b33a6e2b83..76b9253fa94 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -540,6 +540,13 @@ struct wsm_hdr {
 	__le16 id;
 };
 
+#define WSM_TX_SEQ_MAX			(7)
+#define WSM_TX_SEQ(seq)			\
+		((seq & WSM_TX_SEQ_MAX) << 13)
+#define WSM_TX_LINK_ID_MAX		(0x0F)
+#define WSM_TX_LINK_ID(link_id)		\
+		((link_id & WSM_TX_LINK_ID_MAX) << 6)
+
 /* ******************************************************************** */
 /* WSM capcbility							*/
 
@@ -701,6 +708,8 @@ struct wsm_tx_confirm {
 	/* The total time in microseconds that the frame spent in */
 	/* the WLAN device before transmission was started. */
 	/* [out] */ u32 txQueueDelay;
+
+	/* [out]*/ u32 link_id;
 };
 
 /* 3.15 */