cw1200: Add workaround for WFD test case 6.1.10

Adds workaround for WFD test case 6.1.10.
The test engine misbehaves and sends ACTION
(Device Discoverability) frames with the same sequence number which
is then dropped by the firmware.
To get around this we temporarily MAP to SA of the ACTION frame
and sent a reset request to the FW.

ST-Ericsson ID: 372706
ST-Ericsson FOSS-OUT ID: NA
Signed-off-by: Ajitpal.Singh <ajitpal.singh@stericsson.com>

Change-Id: Id1d80920a3d3c57d14a81e771ff5b0dc73a4184d
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/37167
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Tested-by: Ajit Pal SINGH <ajitpal.singh@stericsson.com>

4 files changed, 60 insertions, 4 deletions

diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 1852c734ce5..0ab5d04a054 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -231,6 +231,11 @@ struct cw1200_common {
 
 	/* statistics */
 	struct ieee80211_low_level_stats stats;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	/* Workaround for WFD testcase 6.1.10*/
+	struct work_struct	linkid_reset_work;
+	u8			action_frame_sa[ETH_ALEN];
+#endif
 };
 
 struct cw1200_sta_priv {
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index 1436c6e60f6..02373201114 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -342,6 +342,9 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	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);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	INIT_WORK(&priv->linkid_reset_work, cw1200_link_id_reset);
+#endif
 	init_timer(&priv->mcast_timeout);
 	priv->mcast_timeout.data = (unsigned long)priv;
 	priv->mcast_timeout.function = cw1200_mcast_timeout;
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index cb3537c0dd7..b59ebf6e4cc 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -719,9 +719,15 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	if (WARN_ON(t.queue >= 4 || !t.rate))
 		goto drop;
 
-	ret = cw1200_tx_h_calc_link_ids(priv, &t);
-	if (ret)
-		goto drop;
+	/* Temporary change*/
+	if (unlikely(ieee80211_is_probe_resp(t.hdr->frame_control))) {
+		t.txpriv.raw_link_id =
+				t.txpriv.link_id = 0;
+	} else {
+		ret = cw1200_tx_h_calc_link_ids(priv, &t);
+		if (ret)
+			goto drop;
+	}
 
 	txrx_printk(KERN_DEBUG "[TX] TX %d bytes "
 			"(queue: %d, link_id: %d (%d)).\n",
@@ -976,6 +982,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;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+#endif
 	struct cw1200_link_entry *entry = NULL;
 	unsigned long grace_period;
 	bool early_data = false;
@@ -993,7 +1002,15 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 			early_data = true;
 		entry->timestamp = jiffies;
 	}
-
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	else if ((priv->vif->p2p == WSM_START_MODE_P2P_GO)
+			&& ieee80211_is_action(frame->frame_control)
+			&& (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		txrx_printk(KERN_DEBUG "[RX] Going to MAP&RESET link ID\n");
+		memcpy(&priv->action_frame_sa[0], ieee80211_get_SA(frame), ETH_ALEN);
+		schedule_work(&priv->linkid_reset_work);
+	}
+#endif
 	if (unlikely(arg->status)) {
 		if (arg->status == WSM_STATUS_MICFAILURE) {
 			txrx_printk(KERN_DEBUG "[RX] MIC failure.\n");
@@ -1161,3 +1178,28 @@ int cw1200_upload_keys(struct cw1200_common *priv)
 		}
 	return ret;
 }
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+/* Workaround for WFD test case 6.1.10 */
+void cw1200_link_id_reset(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, linkid_reset_work);
+	int temp_linkid;
+
+	/* In GO mode we can receive ACTION frames without a linkID */
+	temp_linkid = cw1200_alloc_link_id(priv, &priv->action_frame_sa[0]);
+	WARN_ON(!temp_linkid);
+	if (temp_linkid) {
+		/* Make sure we execute the WQ */
+		flush_workqueue(priv->workqueue);
+		/* Release the link ID */
+		spin_lock(&priv->ps_state_lock);
+		priv->link_id_db[temp_linkid - 1].status =
+			CW1200_LINK_RESERVE;
+		spin_unlock(&priv->ps_state_lock);
+		wsm_lock_tx_async(priv);
+		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
+			wsm_unlock_tx(priv);
+	}
+}
+#endif
diff --git a/drivers/staging/cw1200/txrx.h b/drivers/staging/cw1200/txrx.h
index 9f4f40ea31c..f3b2023ff2b 100644
--- a/drivers/staging/cw1200/txrx.h
+++ b/drivers/staging/cw1200/txrx.h
@@ -86,4 +86,10 @@ void cw1200_free_key(struct cw1200_common *priv, int idx);
 void cw1200_free_keys(struct cw1200_common *priv);
 int cw1200_upload_keys(struct cw1200_common *priv);
 
+/* ******************************************************************** */
+/* Workaround for WFD test case 6.1.10					*/
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_link_id_reset(struct work_struct *work);
+#endif
+
 #endif /* CW1200_TXRX_H */