cw1200: Allow TXing in monitor mode

Forced-join firmware mode, previously used for offchannel TXing,
has number of limitations and does not work well.
The patch re-implements offchannel TXing in monitor aka p2p-dev
mode.

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

8 files changed, 97 insertions, 64 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 9a8e41f1e2a..362a5560116 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -351,6 +351,7 @@ void cw1200_bss_info_changed(struct ieee80211_hw *dev,
 			WARN_ON(wsm_set_bss_params(priv, &priv->bss_params));
 			WARN_ON(wsm_set_beacon_wakeup_period(priv,
 				dtim_interval, listen_interval));
+			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
 #if 0
 			/* It's better to override internal TX rete; otherwise
 			 * device sends RTS at too high rate. However device
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 067ad1ed5cd..1c5fb561492 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -157,6 +157,7 @@ struct cw1200_common {
 	struct work_struct	join_work;
 	struct delayed_work	join_timeout;
 	struct work_struct	unjoin_work;
+	struct work_struct	offchannel_work;
 	int			join_dtim_period;
 	bool			delayed_unjoin;
 
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index 74095a59d8a..cddb9943ee6 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -313,6 +313,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	INIT_WORK(&priv->join_work, cw1200_join_work);
 	INIT_DELAYED_WORK(&priv->join_timeout, cw1200_join_timeout);
 	INIT_WORK(&priv->unjoin_work, cw1200_unjoin_work);
+	INIT_WORK(&priv->offchannel_work, cw1200_offchannel_work);
 	INIT_WORK(&priv->wep_key_work, cw1200_wep_key_work);
 	INIT_WORK(&priv->tx_policy_upload_work, tx_policy_upload_work);
 	INIT_LIST_HEAD(&priv->event_queue);
diff --git a/drivers/staging/cw1200/scan.c b/drivers/staging/cw1200/scan.c
index 3c80c571ea7..c663f6dc173 100644
--- a/drivers/staging/cw1200/scan.c
+++ b/drivers/staging/cw1200/scan.c
@@ -12,6 +12,7 @@
 #include <linux/sched.h>
 #include "cw1200.h"
 #include "scan.h"
+#include "sta.h"
 
 static void cw1200_scan_restart_delayed(struct cw1200_common *priv);
 
@@ -95,6 +96,9 @@ int cw1200_hw_scan(struct ieee80211_hw *hw,
 		struct wsm_set_pm pm = priv->powersave_mode;
 		pm.pmMode = WSM_PSM_PS;
 		wsm_set_pm(priv, &pm);
+	} else if (priv->join_status == CW1200_JOIN_STATUS_MONITOR) {
+		/* FW bug: driver has to restart p2p-dev mode after scan */
+		cw1200_disable_listening(priv);
 	}
 
 	BUG_ON(priv->scan.req);
@@ -242,6 +246,13 @@ static void cw1200_scan_restart_delayed(struct cw1200_common *priv)
 				&priv->bss_loss_work,
 				tmo * HZ / 10);
 	}
+
+	/* FW bug: driver has to restart p2p-dev mode after scan. */
+	if (priv->join_status == CW1200_JOIN_STATUS_MONITOR) {
+		cw1200_enable_listening(priv);
+		cw1200_update_filtering(priv);
+	}
+
 	if (priv->delayed_unjoin) {
 		priv->delayed_unjoin = false;
 		if (queue_work(priv->workqueue, &priv->unjoin_work) <= 0)
@@ -258,21 +269,21 @@ static void cw1200_scan_complete(struct cw1200_common *priv)
 		up(&priv->scan.lock);
 		wsm_unlock_tx(priv);
 	} else {
-		queue_work(priv->workqueue, &priv->scan.work);
+		cw1200_scan_work(&priv->scan.work);
 	}
 }
 
 void cw1200_scan_complete_cb(struct cw1200_common *priv,
 				struct wsm_scan_complete *arg)
 {
-	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED))
 		/* STA is stopped. */
 		return;
-	}
 
-	if (likely(atomic_xchg(&priv->scan.in_progress, 0))) {
-		cancel_delayed_work_sync(&priv->scan.timeout);
-		cw1200_scan_complete(priv);
+	if (cancel_delayed_work_sync(&priv->scan.timeout) > 0) {
+		priv->scan.status = 1;
+		queue_delayed_work(priv->workqueue,
+				&priv->scan.timeout, 0);
 	}
 }
 
@@ -281,9 +292,14 @@ void cw1200_scan_timeout(struct work_struct *work)
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, scan.timeout.work);
 	if (likely(atomic_xchg(&priv->scan.in_progress, 0))) {
-		wiphy_warn(priv->hw->wiphy,
-			"Timeout waiting for scan " \
-			"complete notification.\n");
+		if (priv->scan.status > 0)
+			priv->scan.status = 0;
+		else if (!priv->scan.status) {
+			wiphy_warn(priv->hw->wiphy,
+				"Timeout waiting for scan "
+				"complete notification.\n");
+			priv->scan.status = -ETIMEDOUT;
+		}
 		cw1200_scan_complete(priv);
 	}
 }
@@ -367,6 +383,9 @@ void cw1200_probe_work(struct work_struct *work)
 	}
 
 	mutex_lock(&priv->conf_mutex);
+	/* FW bug: driver has to restart p2p-dev mode after scan */
+	if (priv->join_status == CW1200_JOIN_STATUS_MONITOR)
+		cw1200_disable_listening(priv);
 	ret = WARN_ON(wsm_set_template_frame(priv, &frame));
 	priv->scan.direct_probe = 1;
 	if (!ret) {
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index cd0284f3f05..17a4184a360 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -375,6 +375,13 @@ int cw1200_config(struct ieee80211_hw *dev, u32 changed)
 			.disableMoreFlagUsage = true,
 		};
 		wsm_lock_tx(priv);
+		/* Disable p2p-dev mode forced by TX request */
+		if ((priv->join_status == CW1200_JOIN_STATUS_MONITOR) &&
+				(conf->flags & IEEE80211_CONF_IDLE) &&
+				!priv->listening) {
+			cw1200_disable_listening(priv);
+			priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		}
 		WARN_ON(wsm_set_operational_mode(priv, &mode));
 		wsm_unlock_tx(priv);
 	}
@@ -977,6 +984,23 @@ int cw1200_setup_mac(struct cw1200_common *priv)
 	return 0;
 }
 
+void cw1200_offchannel_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, offchannel_work);
+
+	BUG_ON(!priv->channel);
+
+	mutex_lock(&priv->conf_mutex);
+	if (!priv->join_status) {
+		wsm_flush_tx(priv);
+		cw1200_update_listening(priv, true);
+		cw1200_update_filtering(priv);
+	}
+	mutex_unlock(&priv->conf_mutex);
+	wsm_unlock_tx(priv);
+}
+
 void cw1200_join_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
@@ -985,18 +1009,15 @@ void cw1200_join_work(struct work_struct *work)
 	const struct ieee80211_hdr *frame = (struct ieee80211_hdr *)&wsm[1];
 	const u8 *bssid = &frame->addr1[0]; /* AP SSID in a 802.11 frame */
 	struct cfg80211_bss *bss;
-	const u8 *ssidie = NULL;
-	const u8 *dtimie = NULL;
+	const u8 *ssidie;
+	const u8 *dtimie;
 	const struct ieee80211_tim_ie *tim = NULL;
 	u8 queueId;
-	bool action;
 
 	BUG_ON(!wsm);
 	BUG_ON(!priv->channel);
 
 	queueId = wsm_queue_id_to_linux(wsm->queueId);
-	action = ieee80211_is_action(frame->frame_control) ||
-			ieee80211_is_probe_resp(frame->frame_control);
 
 	if (unlikely(priv->join_status)) {
 		wsm_lock_tx(priv);
@@ -1005,43 +1026,37 @@ 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, NULL, bssid, NULL, 0, 0, 0);
-	if (!bss && !action) {
-		priv->join_pending_frame = NULL;
+	if (!bss) {
 		cw1200_queue_remove(&priv->tx_queue[queueId],
 			priv, __le32_to_cpu(wsm->packetID));
 		wsm_unlock_tx(priv);
 		return;
-	} else if (bss) {
-		ssidie = cfg80211_find_ie(WLAN_EID_SSID,
-			bss->information_elements,
-			bss->len_information_elements);
-		dtimie = cfg80211_find_ie(WLAN_EID_TIM,
-			bss->information_elements,
-			bss->len_information_elements);
-		if (dtimie)
-			tim = (struct ieee80211_tim_ie *)&dtimie[2];
 	}
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID,
+		bss->information_elements,
+		bss->len_information_elements);
+	dtimie = cfg80211_find_ie(WLAN_EID_TIM,
+		bss->information_elements,
+		bss->len_information_elements);
+	if (dtimie)
+		tim = (struct ieee80211_tim_ie *)&dtimie[2];
 
 	mutex_lock(&priv->conf_mutex);
 	{
 		struct wsm_join join = {
-			.mode = WSM_JOIN_MODE_BSS,
+			.mode = (bss->capability & WLAN_CAPABILITY_IBSS) ?
+				WSM_JOIN_MODE_IBSS : WSM_JOIN_MODE_BSS,
 			.preambleType = WSM_JOIN_PREAMBLE_SHORT,
 			.probeForJoin = 1,
 			/* dtimPeriod will be updated after association */
 			.dtimPeriod = 1,
-			.beaconInterval = 100,
+			.beaconInterval = bss->beacon_interval,
 			/* basicRateSet will be updated after association */
 			.basicRateSet = 7,
 		};
 
-		if (bss) {
-			join.mode = (bss->capability & WLAN_CAPABILITY_IBSS) ?
-				WSM_JOIN_MODE_IBSS : WSM_JOIN_MODE_BSS;
-			join.beaconInterval = bss->beacon_interval;
-		}
-
 		if (tim && tim->dtim_period > 1) {
 			join.dtimPeriod = tim->dtim_period;
 			priv->join_dtim_period = tim->dtim_period;
@@ -1049,17 +1064,6 @@ void cw1200_join_work(struct work_struct *work)
 				join.dtimPeriod);
 		}
 
-		if (action) {
-			join.probeForJoin = 0;
-			join.flags |= WSM_JOIN_FLAGS_UNSYNCRONIZED |
-					WSM_JOIN_FLAGS_FORCE;
-		} else {
-			WARN_ON(wsm_set_block_ack_policy(priv,
-				priv->ba_tid_mask, priv->ba_tid_mask));
-		}
-
-		priv->join_pending_frame = NULL;
-
 		join.channelNumber = priv->channel->hw_value;
 		join.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
 			WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
@@ -1076,6 +1080,9 @@ void cw1200_join_work(struct work_struct *work)
 
 		wsm_flush_tx(priv);
 
+		WARN_ON(wsm_set_block_ack_policy(priv,
+				priv->ba_tid_mask, priv->ba_tid_mask));
+
 		/* Queue unjoin if not associated in 3 sec. */
 		queue_delayed_work(priv->workqueue,
 			&priv->join_timeout, 3 * HZ);
@@ -1088,19 +1095,19 @@ void cw1200_join_work(struct work_struct *work)
 				priv, __le32_to_cpu(wsm->packetID));
 			cancel_delayed_work_sync(&priv->join_timeout);
 			cw1200_update_listening(priv, priv->listening);
-			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
 		} 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));
+			priv->join_status = CW1200_JOIN_STATUS_STA;
 		}
 		cw1200_update_filtering(priv);
 	}
 	mutex_unlock(&priv->conf_mutex);
-	if (bss)
-		cfg80211_put_bss(bss);
+	cfg80211_put_bss(bss);
+	wsm_unlock_tx(priv);
 }
 
 void cw1200_join_timeout(struct work_struct *work)
@@ -1162,7 +1169,7 @@ void cw1200_unjoin_work(struct work_struct *work)
 	wsm_unlock_tx(priv);
 }
 
-static inline int cw1200_enable_listening(struct cw1200_common *priv)
+int cw1200_enable_listening(struct cw1200_common *priv)
 {
 	struct wsm_start start = {
 		.mode = WSM_START_MODE_P2P_DEV,
@@ -1177,7 +1184,7 @@ static inline int cw1200_enable_listening(struct cw1200_common *priv)
 	return wsm_start(priv, &start);
 }
 
-static inline int cw1200_disable_listening(struct cw1200_common *priv)
+int cw1200_disable_listening(struct cw1200_common *priv)
 {
 	int ret;
 	struct wsm_reset reset = {
diff --git a/drivers/staging/cw1200/sta.h b/drivers/staging/cw1200/sta.h
index 8d0cc5d4c12..88c1cf6a34b 100644
--- a/drivers/staging/cw1200/sta.h
+++ b/drivers/staging/cw1200/sta.h
@@ -69,9 +69,12 @@ int cw1200_setup_mac(struct cw1200_common *priv);
 void cw1200_join_work(struct work_struct *work);
 void cw1200_join_timeout(struct work_struct *work);
 void cw1200_unjoin_work(struct work_struct *work);
+void cw1200_offchannel_work(struct work_struct *work);
 void cw1200_wep_key_work(struct work_struct *work);
 void cw1200_update_listening(struct cw1200_common *priv, bool enabled);
 void cw1200_update_filtering(struct cw1200_common *priv);
 int __cw1200_flush(struct cw1200_common *priv, bool drop);
+int cw1200_enable_listening(struct cw1200_common *priv);
+int cw1200_disable_listening(struct cw1200_common *priv);
 
 #endif
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index f1ab481984b..970b3cd9405 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -423,22 +423,16 @@ static int wsm_join_confirm(struct cw1200_common *priv,
 			    struct wsm_buf *buf)
 {
 	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS)) {
-		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
-		wsm_unlock_tx(priv);
 		return -EINVAL;
 	}
 
 	arg->minPowerLevel = WSM_GET32(buf);
 	arg->maxPowerLevel = WSM_GET32(buf);
 
-	priv->join_status = CW1200_JOIN_STATUS_STA;
-	wsm_unlock_tx(priv);
 	return 0;
 
 underflow:
 	WARN_ON(1);
-	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
-	wsm_unlock_tx(priv);
 	return -EINVAL;
 }
 
@@ -1249,6 +1243,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 		doProbe,
 		doDrop,
 		doJoin,
+		doOffchannel,
 		doWep,
 		doTx,
 	} action = doTx;
@@ -1263,14 +1258,11 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 				action = doJoin;
 			else if (ieee80211_is_probe_req(fctl))
 				action = doTx;
-			else if (ieee80211_is_action(fctl) ||
-					ieee80211_is_probe_resp(fctl)) {
-				if (priv->join_status > CW1200_JOIN_STATUS_MONITOR)
-					action = doTx;
-				else
-					action = doJoin;
-			} else
-				action = doDrop;
+			else if (priv->join_status >=
+					CW1200_JOIN_STATUS_MONITOR)
+				action = doTx;
+			else
+				action = doOffchannel;
 		}
 		break;
 	case NL80211_IFTYPE_AP:
@@ -1364,6 +1356,15 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 		handled = true;
 	}
 	break;
+	case doOffchannel:
+	{
+		wsm_printk(KERN_DEBUG "[WSM] Offchannel TX request.\n");
+		wsm_lock_tx_async(priv);
+		if (queue_work(priv->workqueue, &priv->offchannel_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
 	case doWep:
 	{
 		wsm_printk(KERN_DEBUG "[WSM] Issue set_default_wep_key.\n");
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 66534b940aa..542eededffc 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -1470,7 +1470,7 @@ static inline int wsm_keep_alive_period(struct cw1200_common *priv,
 					int period)
 {
 	struct wsm_keep_alive_period arg = {
-		.keepAlivePeriod = period,
+		.keepAlivePeriod = __cpu_to_le16(period),
 	};
 	return wsm_write_mib(priv, WSM_MIB_ID_KEEP_ALIVE_PERIOD,
 			&arg, sizeof(arg));