cw1200: Send NOA notification after NOA set

Send Notice Of Absence notification to upper layer
after we set NOA params in P2P_GO mode.

ST-Ericsson ID: 401209
ST-Ericsson FOSS-OUT ID: NA

Change-Id: I92bc8ce4d6e75c48a574857f576a9bc1fd7a7da6
Depends-On: I9424401fcd9e69067aebd4fbfd97141d91003b89
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/40156
Tested-by: Janusz DZIEDZIC <janusz.dziedzic@tieto.com>
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>

4 files changed, 74 insertions, 6 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 933b69b27b9..ce82ba5e87f 100644
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -1116,3 +1116,39 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 	wsm_unlock_tx(priv);
 }
 
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_notify_noa(struct cw1200_common *priv, int delay)
+{
+	struct cfg80211_p2p_ps p2p_ps = {0};
+	struct wsm_p2p_ps_modeinfo *modeinfo;
+	modeinfo = &priv->p2p_ps_modeinfo;
+
+	ap_printk(KERN_DEBUG "[AP]: %s called\n", __func__);
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		return;
+
+	if (delay)
+		msleep(delay);
+
+	if (!WARN_ON(wsm_get_p2p_ps_modeinfo(priv, modeinfo))) {
+#if defined(CONFIG_CW1200_STA_DEBUG)
+		print_hex_dump_bytes("[AP] p2p_get_ps_modeinfo: ",
+				     DUMP_PREFIX_NONE,
+				     (u8 *)modeinfo,
+				     sizeof(*modeinfo));
+#endif /* CONFIG_CW1200_STA_DEBUG */
+		p2p_ps.opp_ps = !!(modeinfo->oppPsCTWindow & BIT(7));
+		p2p_ps.ctwindow = modeinfo->oppPsCTWindow & (~BIT(7));
+		p2p_ps.count = modeinfo->count;
+		p2p_ps.start = __le32_to_cpu(modeinfo->startTime);
+		p2p_ps.duration = __le32_to_cpu(modeinfo->duration);
+		p2p_ps.interval = __le32_to_cpu(modeinfo->interval);
+		p2p_ps.index = modeinfo->reserved;
+
+		ieee80211_p2p_noa_notify(priv->vif,
+					 &p2p_ps,
+					 GFP_KERNEL);
+	}
+}
+#endif
diff --git a/drivers/staging/cw1200/ap.h b/drivers/staging/cw1200/ap.h
index 2175a82d2a2..c10e4ef16d2 100644
--- a/drivers/staging/cw1200/ap.h
+++ b/drivers/staging/cw1200/ap.h
@@ -42,5 +42,8 @@ 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);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_notify_noa(struct cw1200_common *priv, int delay);
+#endif
 
 #endif
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index ad85f29008a..85e57d2f7b3 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -15,6 +15,7 @@
 
 #include "cw1200.h"
 #include "sta.h"
+#include "ap.h"
 #include "fwio.h"
 #include "bh.h"
 #include "debug.h"
@@ -363,17 +364,34 @@ int cw1200_config(struct ieee80211_hw *dev, u32 changed)
 			conf->p2p_ps.interval);
 		/* Notice of Absence */
 		modeinfo->count = conf->p2p_ps.count;
-		modeinfo->startTime = __cpu_to_le32(conf->p2p_ps.start);
-		modeinfo->duration = __cpu_to_le32(conf->p2p_ps.duration);
-		modeinfo->interval = __cpu_to_le32(conf->p2p_ps.interval);
 
-		if (conf->p2p_ps.count)
+		if (conf->p2p_ps.count) {
+			/* In case P2P_GO we need some extra time to be sure
+			 * we will update beacon/probe_resp IEs correctly */
+#define NOA_DELAY_START_MS	300
+			if (priv->join_status == CW1200_JOIN_STATUS_AP)
+				modeinfo->startTime =
+					__cpu_to_le32(conf->p2p_ps.start +
+						      NOA_DELAY_START_MS);
+			else
+				modeinfo->startTime =
+					__cpu_to_le32(conf->p2p_ps.start);
+			modeinfo->duration =
+				__cpu_to_le32(conf->p2p_ps.duration);
+			modeinfo->interval =
+				 __cpu_to_le32(conf->p2p_ps.interval);
 			modeinfo->dtimCount = 1;
-		else
+			modeinfo->reserved = 0;
+		} else {
 			modeinfo->dtimCount = 0;
+			modeinfo->startTime = 0;
+			modeinfo->reserved = 0;
+			modeinfo->duration = 0;
+			modeinfo->interval = 0;
+		}
 
 #if defined(CONFIG_CW1200_STA_DEBUG)
-		print_hex_dump_bytes("p2p_ps_modeinfo: ",
+		print_hex_dump_bytes("p2p_set_ps_modeinfo: ",
 				     DUMP_PREFIX_NONE,
 				     (u8 *)modeinfo,
 				     sizeof(*modeinfo));
@@ -382,6 +400,10 @@ int cw1200_config(struct ieee80211_hw *dev, u32 changed)
 		    priv->join_status == CW1200_JOIN_STATUS_AP) {
 			WARN_ON(wsm_set_p2p_ps_modeinfo(priv, modeinfo));
 		}
+
+		/* Temporary solution while firmware don't support NOA change
+		 * notification yet */
+		cw1200_notify_noa(priv, 10);
 	}
 #endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
 
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 61b2bd0c06b..004a55f1240 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -1658,6 +1658,13 @@ static inline int wsm_set_p2p_ps_modeinfo(struct cw1200_common *priv,
 			     mi, sizeof(*mi));
 }
 
+static inline int wsm_get_p2p_ps_modeinfo(struct cw1200_common *priv,
+					  struct wsm_p2p_ps_modeinfo *mi)
+{
+	return wsm_read_mib(priv, WSM_MIB_ID_P2P_PS_MODE_INFO,
+			    mi, sizeof(*mi));
+}
+
 /* UseMultiTxConfMessage */
 
 static inline int wsm_use_multi_tx_conf(struct cw1200_common *priv,