cw1200: Add BT Coexistence support

BT Coexistence support required setting priority to frames for
PTA arbitration FW, parsing SDD file for getting the listen
interval and using the same in assoc request, modifying Tx rate
for PSPOLL and NULL templates (plumbing them)

ST-Ericsson ID: 357776
ST-Ericsson FOSS-OUT ID: STETL-FOSS-OUT-10091

Change-Id: I14f05cbcc2f02b85f72dbe820893cef9c3775df7
Signed-off-by: Amit Shakya <amit.shakya@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/31602
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33528
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>
Tested-by: Dmitry TARNYAGIN <dmitry.tarnyagin@stericsson.com>

8 files changed, 282 insertions, 8 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 494403dd28d..aabca5ca06e 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -23,6 +23,8 @@
 #define CW1200_LINK_ID_GC_TIMEOUT ((unsigned long)(10 * HZ))
 
 static int cw1200_upload_beacon(struct cw1200_common *priv);
+static int cw1200_upload_pspoll(struct cw1200_common *priv);
+static int cw1200_upload_null(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,
@@ -195,6 +197,56 @@ int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
 	return 0;
 }
 
+static int cw1200_set_btcoexinfo(struct cw1200_common *priv)
+{
+	struct wsm_override_internal_txrate arg;
+	int ret = 0;
+
+	if (priv->mode == NL80211_IFTYPE_STATION) {
+		/* Plumb PSPOLL and NULL template */
+		WARN_ON(cw1200_upload_pspoll(priv));
+		WARN_ON(cw1200_upload_null(priv));
+	} else {
+		return 0;
+	}
+
+	memset(&arg, 0, sizeof(struct wsm_override_internal_txrate));
+
+	if (!priv->vif->p2p) {
+		/* STATION mode */
+		if (priv->bss_params.operationalRateSet & ~0xF) {
+			ap_printk(KERN_DEBUG "[STA] STA has ERP rates\n");
+			/* G or BG mode */
+			arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		} else {
+			ap_printk(KERN_DEBUG "[STA] STA has non ERP rates\n");
+			/* B only mode */
+			arg.internalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+		}
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+	} else {
+		/* P2P mode */
+		arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+	}
+
+	ap_printk(KERN_DEBUG "[STA] BTCOEX_INFO"
+		"MODE %d, internalTxRate : %x, nonErpInternalTxRate: %x\n",
+		priv->mode,
+		arg.internalTxRate,
+		arg.nonErpInternalTxRate);
+
+	ret = WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+		&arg, sizeof(arg)));
+
+	return ret;
+}
+
 void cw1200_bss_info_changed(struct ieee80211_hw *dev,
 			     struct ieee80211_vif *vif,
 			     struct ieee80211_bss_conf *info,
@@ -389,6 +441,9 @@ void cw1200_bss_info_changed(struct ieee80211_hw *dev,
 			WARN_ON(wsm_set_beacon_wakeup_period(priv,
 				dtim_interval, listen_interval));
 			cw1200_set_pm(priv, &priv->powersave_mode);
+
+			if (priv->is_BT_Present)
+				WARN_ON(cw1200_set_btcoexinfo(priv));
 #if 0
 			/* It's better to override internal TX rete; otherwise
 			 * device sends RTS at too high rate. However device
@@ -683,6 +738,46 @@ static int cw1200_upload_beacon(struct cw1200_common *priv)
 	return ret;
 }
 
+static int cw1200_upload_pspoll(struct cw1200_common *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PS_POLL,
+		.rate = 0xFF,
+	};
+
+
+	frame.skb = ieee80211_pspoll_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	ret = wsm_set_template_frame(priv, &frame);
+
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+static int cw1200_upload_null(struct cw1200_common *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_NULL,
+		.rate = 0xFF,
+	};
+
+
+	frame.skb = ieee80211_nullfunc_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	ret = wsm_set_template_frame(priv, &frame);
+
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
 static int cw1200_enable_beaconing(struct cw1200_common *priv,
 				   bool enable)
 {
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 85d7caf89e9..400fd4a44aa 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -133,8 +133,11 @@ struct cw1200_common {
 	struct wsm_multicast_filter	multicast_filter;
 	struct cw1200_pm_state		pm_state;
 	struct wsm_p2p_ps_modeinfo	p2p_ps_modeinfo;
-	struct wsm_uapsd_info           uapsd_info;
-	bool                            setbssparams_done;
+	struct wsm_uapsd_info		uapsd_info;
+	bool				setbssparams_done;
+	bool				is_BT_Present;
+	u8				conf_listen_interval;
+	u32				listen_interval;
 
 	/* BH */
 	atomic_t			bh_rx;
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index c6eabd12c56..e09984583f6 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -199,14 +199,15 @@ size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 }
 
 int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
-			struct sk_buff *skb, u8 link_id)
+			struct sk_buff *skb, struct tx_info *txinfo)
 {
 	int ret;
 	struct wsm_tx *wsm;
 	struct cw1200_queue_stats *stats = queue->stats;
+	u8	link_id = txinfo->link_id;
 
 	wsm = (struct wsm_tx *)skb_push(skb, sizeof(struct wsm_tx));
-	ret = cw1200_skb_to_wsm(priv, skb, wsm);
+	ret = cw1200_skb_to_wsm(priv, skb, wsm, txinfo);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index b28d0294d25..4abea64aa66 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -18,6 +18,7 @@
 /* extern */ struct wsm_tx;
 /* extern */ struct cw1200_common;
 /* extern */ struct ieee80211_tx_queue_stats;
+/* extern */ struct tx_info;
 
 /* forward */ struct cw1200_queue_stats;
 
@@ -60,7 +61,7 @@ void cw1200_queue_deinit(struct cw1200_queue *queue);
 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 				   u32 link_id_map);
 int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *cw1200,
-			struct sk_buff *skb, u8 link_id);
+			struct sk_buff *skb,  struct tx_info *txinfo);
 int cw1200_queue_get(struct cw1200_queue *queue,
 		     u32 link_id_map,
 		     struct wsm_tx **tx,
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index 82c410c6c88..4aa320d76d9 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -945,6 +945,66 @@ void cw1200_tx_failure_work(struct work_struct *work)
 /* ******************************************************************** */
 /* Internal API								*/
 
+
+
+/*
+* This function is called to Parse the SDD file
+ *to extract listen_interval and PTA related information
+*/
+static int cw1200_parse_SDD_file(struct cw1200_common *priv)
+{
+	u8 *sdd_data = (u8 *)priv->sdd->data;
+	struct cw1200_sdd {
+		u8 id ;
+		u8 length ;
+		u8 data[] ;
+	} *pElement;
+	int parsedLength = 0;
+	#define SDD_PTA_CFG_ELT_ID 0xEB
+	#define FIELD_OFFSET(type, field) ((u8 *)&((type*)0)->field - (u8 *)0)
+
+	priv->is_BT_Present = false;
+
+	pElement = (struct cw1200_sdd *)sdd_data;
+
+	pElement = (struct cw1200_sdd *)((u8*)pElement +
+		FIELD_OFFSET(struct cw1200_sdd, data) + pElement->length);
+
+	parsedLength += (FIELD_OFFSET(struct cw1200_sdd, data) +
+			pElement->length);
+
+	while (parsedLength <= priv->sdd->size) {
+		switch (pElement->id) {
+		case SDD_PTA_CFG_ELT_ID:
+		{
+			priv->conf_listen_interval =
+				(*((u16 *)pElement->data+1) >> 7) & 0x1F;
+			priv->is_BT_Present = true;
+			sta_printk(KERN_DEBUG "PTA element found.\n");
+			sta_printk(KERN_DEBUG "Listen Interval %d\n",
+						priv->conf_listen_interval);
+		}
+		break;
+
+		default:
+		break;
+		}
+
+		pElement = (struct cw1200_sdd *)
+			((u8 *)pElement + FIELD_OFFSET(struct cw1200_sdd, data)
+					+ pElement->length);
+		parsedLength +=
+		(FIELD_OFFSET(struct cw1200_sdd, data) + pElement->length);
+	}
+
+	if (priv->is_BT_Present == false) {
+		sta_printk(KERN_DEBUG "PTA element NOT found.\n");
+		priv->conf_listen_interval = 0;
+	}
+	return 0;
+}
+
+
 int cw1200_setup_mac(struct cw1200_common *priv)
 {
 	/* TBD: Do you know how to assing MAC address without
@@ -1002,6 +1062,8 @@ int cw1200_setup_mac(struct cw1200_common *priv)
 		cfg.dpdData = priv->sdd->data;
 		cfg.dpdData_size = priv->sdd->size;
 		ret = WARN_ON(wsm_configuration(priv, &cfg));
+		/* Parse SDD file for PTA element */
+		cw1200_parse_SDD_file(priv);
 	}
 	if (ret)
 		return ret;
@@ -1099,6 +1161,19 @@ void cw1200_join_work(struct work_struct *work)
 			.basicRateSet = 7,
 		};
 
+		/* BT Coex related changes */
+		if (priv->is_BT_Present) {
+			if (((priv->conf_listen_interval * 100) %
+					bss->beacon_interval) == 0)
+				priv->listen_interval =
+					((priv->conf_listen_interval * 100) /
+					bss->beacon_interval);
+			else
+				priv->listen_interval =
+					((priv->conf_listen_interval * 100) /
+					bss->beacon_interval + 1);
+		}
+
 		if (tim && tim->dtim_period > 1) {
 			join.dtimPeriod = tim->dtim_period;
 			priv->join_dtim_period = tim->dtim_period;
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 1742af0ac8e..3e8b84fa43c 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -371,12 +371,13 @@ cw1200_get_tx_rate(const struct cw1200_common *priv,
 
 /* NOTE: cw1200_skb_to_wsm executes in atomic context. */
 int cw1200_skb_to_wsm(struct cw1200_common *priv, struct sk_buff *skb,
-			struct wsm_tx *wsm)
+			struct wsm_tx *wsm,  struct tx_info *txinfo)
 {
 	bool tx_policy_renew = false;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	const struct ieee80211_rate *rate = cw1200_get_tx_rate(priv,
 		&tx_info->control.rates[0]);
+	u8 priority = 0;
 
 	memset(wsm, 0, sizeof(*wsm));
 	wsm->hdr.len = __cpu_to_le16(skb->len);
@@ -409,6 +410,32 @@ int cw1200_skb_to_wsm(struct cw1200_common *priv, struct sk_buff *skb,
 	}
 
 	wsm->queueId = wsm_queue_id_to_wsm(skb_get_queue_mapping(skb));
+
+	/* BT Coex specific handling */
+	if (priv->is_BT_Present) {
+		struct ieee80211_hdr *hdr =
+		(struct ieee80211_hdr *)(skb->data + sizeof(struct wsm_tx));
+
+		if (cpu_to_be16(txinfo->ethertype) == ETH_P_PAE)
+			priority = WSM_EPTA_PRIORITY_EAPOL;
+		else if (ieee80211_is_action(hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_ACTION;
+		else if (ieee80211_is_mgmt(hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_MGT;
+		else if ((wsm->queueId == WSM_QUEUE_VOICE))
+			priority = WSM_EPTA_PRIORITY_VOICE;
+		else if ((wsm->queueId == WSM_QUEUE_VIDEO))
+			priority = WSM_EPTA_PRIORITY_VIDEO;
+		else
+			priority = WSM_EPTA_PRIORITY_DATA;
+
+		txrx_printk(KERN_DEBUG "[TX] EPTA priority %x.\n",
+			((priority) & 0x7));
+
+		/* Set EPTA priority */
+		wsm->flags |= (((priority) & 0x7) << 1);
+	}
+
 	return 0;
 }
 
@@ -427,6 +454,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	int link_id;
 	int ret;
 	int i;
+	struct tx_info txinfo;
 
 	if (likely(tx_info->control.sta && sta_priv->link_id))
 		link_id = sta_priv->link_id;
@@ -465,6 +493,39 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		spin_unlock_bh(&priv->buffered_multicasts_lock);
 	}
 
+	/* BT Coex support related configuration */
+	if (priv->is_BT_Present) {
+		txinfo.ethertype = 0;
+
+		if (ieee80211_is_data_qos(hdr->frame_control) ||
+			ieee80211_is_data(hdr->frame_control)) {
+			unsigned int headerlen =
+				ieee80211_get_hdrlen_from_skb(skb);
+
+			/* Skip LLC SNAP header (+6) */
+			if (headerlen > 0)
+				txinfo.ethertype =
+					*((u16 *)(skb->data + headerlen + 6));
+		}
+		else if (ieee80211_is_assoc_req(hdr->frame_control) ||
+			ieee80211_is_reassoc_req(hdr->frame_control)) {
+			struct ieee80211_mgmt *mgt_frame =
+					(struct ieee80211_mgmt *)skb->data;
+
+			if (mgt_frame->u.assoc_req.listen_interval <
+							priv->listen_interval) {
+				txrx_printk(KERN_DEBUG
+				"Modified Listen Interval to %x from %x\n",
+				priv->listen_interval,
+				mgt_frame->u.assoc_req.listen_interval);
+				/* Replace listen interval derieved from
+				the one read from SDD */
+				mgt_frame->u.assoc_req.listen_interval =
+					priv->listen_interval;
+			}
+		}
+	}
+
 	/* IV/ICV injection. */
 	/* TODO: Quite unoptimal. It's better co modify mac80211
 	 * to reserve space for IV */
@@ -543,8 +604,10 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 			queue_work(priv->workqueue,
 				&priv->multicast_start_work);
 	}
+
+	txinfo.link_id = link_id;
 	ret = cw1200_queue_put(&priv->tx_queue[queue], priv, skb,
-			link_id);
+			&txinfo);
 	spin_unlock_bh(&priv->buffered_multicasts_lock);
 
 	if (!WARN_ON(ret))
diff --git a/drivers/staging/cw1200/txrx.h b/drivers/staging/cw1200/txrx.h
index 12ec3d9808e..e41ac43bc7f 100644
--- a/drivers/staging/cw1200/txrx.h
+++ b/drivers/staging/cw1200/txrx.h
@@ -43,6 +43,11 @@ struct tx_policy_cache {
 	spinlock_t lock;
 };
 
+struct tx_info {
+	u8	link_id;
+	u16	ethertype;
+};
+
 /* ******************************************************************** */
 /* TX policy cache							*/
 /* Intention of TX policy cache is an overcomplicated WSM API.
@@ -60,7 +65,8 @@ void tx_policy_put(struct cw1200_common *priv, int idx);
 u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv,
 			       u32 rates);
 int cw1200_skb_to_wsm(struct cw1200_common *priv,
-		      struct sk_buff *skb, struct wsm_tx *wsm);
+		      struct sk_buff *skb, struct wsm_tx *wsm,
+			struct tx_info *txinfo);
 void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb);
 
 /* ******************************************************************** */
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index e9f7c922994..adb34765c5c 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -173,6 +173,22 @@ struct cw1200_common;
 /* STBC allowed */
 #define WSM_HT_TX_STBC			(BIT(7))
 
+/* EPTA prioirty flags for BT Coex */
+/* default epta priority */
+#define WSM_EPTA_PRIORITY_DEFAULT	4
+/* use for normal data */
+#define WSM_EPTA_PRIORITY_DATA		4
+/* use for connect/disconnect/roaming*/
+#define WSM_EPTA_PRIORITY_MGT		5
+/* use for action frames */
+#define WSM_EPTA_PRIORITY_ACTION	5
+/* use for AC_VI data */
+#define WSM_EPTA_PRIORITY_VIDEO		5
+/* use for AC_VO data */
+#define WSM_EPTA_PRIORITY_VOICE		6
+/* use for EAPOL exchange */
+#define WSM_EPTA_PRIORITY_EAPOL		7
+
 /* TX status */
 /* Frame was sent aggregated */
 /* Only valid for WSM_SUCCESS status. */
@@ -1592,6 +1608,20 @@ static inline int wsm_set_uapsd_info(struct cw1200_common *priv,
 				arg, sizeof(*arg));
 }
 
+/* 4.22 OverrideInternalTxRate */
+struct wsm_override_internal_txrate {
+	u8 internalTxRate;
+	u8 nonErpInternalTxRate;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_set_override_internal_txrate(struct cw1200_common *priv,
+				     struct wsm_override_internal_txrate *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+				arg, sizeof(*arg));
+}
+
 /* ******************************************************************** */
 /* WSM TX port control							*/