cw1200: P2P power save support

P2P power save support:
	- Opportunistic Power Save
	- Notice Of Absence

Change-Id: I5523c852632598d9c80acc390c920ccfb42eaefd
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/28267
Tested-by: Janusz DZIEDZIC <janusz.dziedzic@tieto.com>
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33500
Tested-by: Dmitry TARNYAGIN <dmitry.tarnyagin@stericsson.com>
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

5 files changed, 157 insertions, 22 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 4f9afde2df5..9a8e41f1e2a 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -23,7 +23,8 @@
 static int cw1200_upload_beacon(struct cw1200_common *priv);
 static int cw1200_start_ap(struct cw1200_common *priv);
 static int cw1200_update_beaconing(struct cw1200_common *priv);
-
+static int cw1200_enable_beaconing(struct cw1200_common *priv,
+				   bool enable);
 
 /* ******************************************************************** */
 /* AP API								*/
@@ -205,15 +206,30 @@ void cw1200_bss_info_changed(struct ieee80211_hw *dev,
 			WARN_ON(1);
 	}
 
-	if (changed & BSS_CHANGED_BEACON_ENABLED)
-		priv->enable_beacon = info->enable_beacon;
 
-	if (changed & BSS_CHANGED_BEACON)
+	if (changed & BSS_CHANGED_BEACON) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_BEACON\n");
+		WARN_ON(cw1200_update_beaconing(priv));
 		WARN_ON(cw1200_upload_beacon(priv));
+	}
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_BEACON_ENABLED\n");
+
+		if (priv->enable_beacon != info->enable_beacon) {
+			WARN_ON(cw1200_enable_beaconing(priv,
+							info->enable_beacon));
+			priv->enable_beacon = info->enable_beacon;
+		}
+	}
+
+	if (changed & BSS_CHANGED_BEACON_INT) {
+		ap_printk(KERN_DEBUG "CHANGED_BEACON_INT\n");
+		/* Restart AP only when connected */
+		if(priv->join_status == CW1200_JOIN_STATUS_AP)
+			WARN_ON(cw1200_update_beaconing(priv));
+	}
 
-	if (changed & (BSS_CHANGED_BEACON_ENABLED | BSS_CHANGED_BEACON |
-			BSS_CHANGED_BEACON_INT))
-		WARN_ON(cw1200_update_beaconing(priv));
 
 	if (changed & BSS_CHANGED_ASSOC) {
 		wsm_lock_tx(priv);
@@ -562,7 +578,6 @@ static int cw1200_upload_beacon(struct cw1200_common *priv)
 		.frame_type = WSM_FRAME_TYPE_BEACON,
 	};
 
-	ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
 
 	frame.skb = ieee80211_beacon_get(priv->hw, priv->vif);
 	if (WARN_ON(!frame.skb))
@@ -597,6 +612,16 @@ static int cw1200_upload_beacon(struct cw1200_common *priv)
 	return ret;
 }
 
+static int cw1200_enable_beaconing(struct cw1200_common *priv,
+				   bool enable)
+{
+	struct wsm_beacon_transmit transmit = {
+		.enableBeaconing = enable,
+	};
+
+	return wsm_beacon_transmit(priv, &transmit);
+}
+
 static int cw1200_start_ap(struct cw1200_common *priv)
 {
 	int ret;
@@ -616,23 +641,18 @@ static int cw1200_start_ap(struct cw1200_common *priv)
 				conf->basic_rates),
 		.ssidLength = priv->ssid_length,
 	};
-	struct wsm_beacon_transmit transmit = {
-		.enableBeaconing = priv->enable_beacon,
-	};
+	priv->beacon_int = conf->beacon_int;
 
 	memcpy(&start.ssid[0], priv->ssid, start.ssidLength);
 
-	ap_printk(KERN_DEBUG "[AP] ch: %d(%d), bcn: %d(%d), brt: 0x%.8X, ssid: %.*s %s.\n",
+	ap_printk(KERN_DEBUG "[AP] ch: %d(%d), bcn: %d(%d), brt: 0x%.8X, ssid: %.*s.\n",
 		start.channelNumber, start.band,
 		start.beaconInterval, start.DTIMPeriod,
 		start.basicRateSet,
-		start.ssidLength, start.ssid,
-		transmit.enableBeaconing ? "ena" : "dis");
+		start.ssidLength, start.ssid);
 	ret = WARN_ON(wsm_start(priv, &start));
 	if (!ret)
 		ret = WARN_ON(cw1200_upload_keys(priv));
-	if (!ret)
-		ret = WARN_ON(wsm_beacon_transmit(priv, &transmit));
 	if (!ret) {
 		WARN_ON(wsm_set_block_ack_policy(priv,
 			priv->ba_tid_mask, priv->ba_tid_mask));
@@ -644,16 +664,24 @@ static int cw1200_start_ap(struct cw1200_common *priv)
 
 static int cw1200_update_beaconing(struct cw1200_common *priv)
 {
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
 	struct wsm_reset reset = {
 		.link_id = 0,
 		.reset_statistics = true,
 	};
 
 	if (priv->mode == NL80211_IFTYPE_AP) {
-		ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
-		WARN_ON(wsm_reset(priv, &reset));
-		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
-		WARN_ON(cw1200_start_ap(priv));
+		/* TODO: check if changed channel, band */
+		if (priv->join_status != CW1200_JOIN_STATUS_AP ||
+		    priv->beacon_int != conf->beacon_int) {
+			ap_printk(KERN_DEBUG "ap restarting\n");
+			if (priv->join_status != CW1200_JOIN_STATUS_PASSIVE)
+				WARN_ON(wsm_reset(priv, &reset));
+			priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+			WARN_ON(cw1200_start_ap(priv));
+		} else
+			ap_printk(KERN_DEBUG "ap started join_status: %d\n",
+				  priv->join_status);
 	}
 	return 0;
 }
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 13c3b2d3751..d0c2d7588d3 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -106,14 +106,17 @@ struct cw1200_common {
 	u8				short_frame_max_tx_count;
 	int				mode;
 	bool				enable_beacon;
+	int				beacon_int;
 	size_t				ssid_length;
 	u8				ssid[IEEE80211_MAX_SSID_LEN];
 	bool				listening;
 	struct wsm_rx_filter		rx_filter;
+	struct wsm_beacon_filter_table	bf_table;
 	struct wsm_beacon_filter_control bf_control;
 	u8				ba_tid_mask;
 	struct wsm_multicast_filter	multicast_filter;
 	struct cw1200_suspend_state	*suspend_state;
+	struct wsm_p2p_ps_modeinfo	p2p_ps_modeinfo;
 
 	/* BH */
 	atomic_t			bh_rx;
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index a704d12d469..74095a59d8a 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -257,6 +257,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 		    /* Aggregation is fully controlled by firmware.
 		     * Do not need any support from the mac80211 stack */
 		    /* IEEE80211_HW_AMPDU_AGGREGATION | */
+		    IEEE80211_HW_SUPPORTS_P2P_PS |
 #if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
 		    IEEE80211_HW_SUPPORTS_CQM_BEACON_MISS |
 		    IEEE80211_HW_SUPPORTS_CQM_TX_FAIL |
@@ -266,7 +267,9 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 					  BIT(NL80211_IFTYPE_ADHOC) |
 					  BIT(NL80211_IFTYPE_AP) |
-					  BIT(NL80211_IFTYPE_MESH_POINT);
+					  BIT(NL80211_IFTYPE_MESH_POINT) |
+					  BIT(NL80211_IFTYPE_P2P_CLIENT) |
+					  BIT(NL80211_IFTYPE_P2P_GO);
 
 	/* Support only for limited wowlan functionalities */
 	hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY |
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index 8c813282021..a0f65c37c18 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -67,6 +67,17 @@ int cw1200_start(struct ieee80211_hw *dev)
 	priv->cqm_link_loss_count = 60;
 	priv->cqm_beacon_loss_count = 20;
 
+	/* Temporary configuration - beacon filter table */
+	priv->bf_table.numOfIEs = __cpu_to_le32(1);
+	priv->bf_table.entry[0].ieId = WLAN_EID_VENDOR_SPECIFIC;
+	priv->bf_table.entry[0].actionFlags = WSM_BEACON_FILTER_IE_HAS_CHANGED |
+					WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+					WSM_BEACON_FILTER_IE_HAS_APPEARED;
+	priv->bf_table.entry[0].oui[0] = 0x50;
+	priv->bf_table.entry[0].oui[1] = 0x6F;
+	priv->bf_table.entry[0].oui[2] = 0x9A;
+
+	priv->bf_control.enabled = 1;
 	ret = cw1200_setup_mac(priv);
 	if (WARN_ON(ret))
 		goto out;
@@ -304,6 +315,50 @@ int cw1200_config(struct ieee80211_hw *dev, u32 changed)
 			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
 	}
 
+	if (changed & IEEE80211_CONF_CHANGE_P2P_PS) {
+		struct wsm_p2p_ps_modeinfo *modeinfo;
+		modeinfo = &priv->p2p_ps_modeinfo;
+		sta_printk(KERN_DEBUG "[STA] IEEE80211_CONF_CHANGE_P2P_PS\n");
+
+		if (conf->p2p_ps.ctwindow >= 128)
+			modeinfo->oppPsCTWindow = 127;
+		else if (conf->p2p_ps.ctwindow >= 0)
+			modeinfo->oppPsCTWindow = conf->p2p_ps.ctwindow;
+
+		switch (conf->p2p_ps.opp_ps) {
+			case 0:
+				modeinfo->oppPsCTWindow &= ~(BIT(7));
+				break;
+			case 1:
+				modeinfo->oppPsCTWindow |= BIT(7);
+				break;
+			default:
+				break;
+		}
+
+		/* 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)
+			modeinfo->dtimCount = 1;
+		else
+			modeinfo->dtimCount = 0;
+
+		if (priv->join_status == CW1200_JOIN_STATUS_STA ||
+		    priv->join_status == CW1200_JOIN_STATUS_AP) {
+#if defined(CONFIG_CW1200_STA_DEBUG)
+			print_hex_dump_bytes("p2p_ps_modeinfo: ",
+					     DUMP_PREFIX_NONE,
+					     (u8*) modeinfo,
+					     sizeof(*modeinfo));
+#endif
+			WARN_ON(wsm_set_p2p_ps_modeinfo(priv, modeinfo));
+		}
+	}
+
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
 		/* TBD: It looks like it's transparent
 		 * there's a monitor interface present -- use this
@@ -353,6 +408,8 @@ void cw1200_update_filtering(struct cw1200_common *priv)
 
 	ret = wsm_set_rx_filter(priv, &priv->rx_filter);
 	if (!ret)
+		ret = wsm_set_beacon_filter_table(priv, &priv->bf_table);
+	if (!ret)
 		ret = wsm_beacon_filter_control(priv, &priv->bf_control);
 	if (!ret)
 		ret = wsm_set_bssid_filtering(priv, !priv->rx_filter.bssid);
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 55792c4b3c8..66534b940aa 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -1246,6 +1246,32 @@ static inline int wsm_set_rx_filter(struct cw1200_common *priv,
 	return wsm_write_mib(priv, WSM_MIB_ID_RX_FILTER, &val, sizeof(val));
 }
 
+#define WSM_BEACON_FILTER_IE_HAS_CHANGED	BIT(0)
+#define WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT	BIT(1)
+#define WSM_BEACON_FILTER_IE_HAS_APPEARED	BIT(2)
+
+struct wsm_beacon_filter_table_entry {
+	u8	ieId;
+	u8	actionFlags;
+	u8	oui[3];
+	u8	matchData[3];
+} __packed;
+
+struct wsm_beacon_filter_table {
+	__le32 numOfIEs;
+	struct wsm_beacon_filter_table_entry entry[10];
+} __packed;
+
+static inline int wsm_set_beacon_filter_table(struct cw1200_common *priv,
+					struct wsm_beacon_filter_table *ft)
+{
+	size_t size = __le32_to_cpu(ft->numOfIEs) *
+		     sizeof(struct wsm_beacon_filter_table_entry) +
+		     sizeof(__le32);
+
+	return wsm_write_mib(priv, WSM_MIB_ID_BEACON_FILTER_TABLE, ft, size);
+}
+
 struct wsm_beacon_filter_control {
 	int enabled;
 	int bcn_count;
@@ -1480,7 +1506,7 @@ static inline int wsm_set_multicast_filter(struct cw1200_common *priv,
 			     fp, sizeof(*fp));
 }
 
-/* IPv4 filtering - 4.10 */
+/* ARP IPv4 filtering - 4.10 */
 struct wsm_arp_ipv4_filter {
 	__le32 enable;
 	__be32 ipv4Address[WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES];
@@ -1493,6 +1519,24 @@ static inline int wsm_set_arp_ipv4_filter(struct cw1200_common *priv,
 			    fp, sizeof(*fp));
 }
 
+/* P2P Power Save Mode Info - 4.31 */
+struct wsm_p2p_ps_modeinfo {
+	u8	oppPsCTWindow;
+	u8	count;
+	u8	reserved;
+	u8	dtimCount;
+	__le32	duration;
+	__le32	interval;
+	__le32	startTime;
+} __packed;
+
+static inline int wsm_set_p2p_ps_modeinfo(struct cw1200_common *priv,
+					  struct wsm_p2p_ps_modeinfo *mi)
+{
+	return wsm_write_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,