cw1200: Add 2byte hole in TXreq for unaligned buf

Adds a 2 bytes hole in the WSM Transmit request when skb->data is
aligned at 2 bytes.This will make the transmit request 4byte aligned.
This optmisation is need for DMA.
The firmware is informed about the hole by setting BIT7 in the
WSM Transmit Request flags.

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

Change-Id: Idbb60459ca645575c2afc74f2421b0fa86121cf0
Signed-off-by: Ajitpal.Singh <ajitpal.singh@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33668
Tested-by: Dmitry TARNYAGIN <dmitry.tarnyagin@stericsson.com>
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

5 files changed, 35 insertions, 22 deletions

diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index 91c690ad4e6..297b83e1ef5 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -288,8 +288,8 @@ static int cw1200_status_show(struct seq_file *seq, void *v)
 		d->rx_agg);
 	seq_printf(seq, "TX miss:    %d\n",
 		d->tx_cache_miss);
-	seq_printf(seq, "TX copy:    %d\n",
-		d->tx_copy);
+	seq_printf(seq, "TX align:   %d\n",
+		d->tx_align);
 	seq_printf(seq, "TX TTL:     %d\n",
 		d->tx_ttl);
 	seq_printf(seq, "Scan:       %s\n",
diff --git a/drivers/staging/cw1200/debug.h b/drivers/staging/cw1200/debug.h
index 6f7d8acab00..aab0c061648 100644
--- a/drivers/staging/cw1200/debug.h
+++ b/drivers/staging/cw1200/debug.h
@@ -14,7 +14,7 @@ struct cw1200_debug_priv {
 	int tx_multi;
 	int tx_multi_frames;
 	int tx_cache_miss;
-	int tx_copy;
+	int tx_align;
 	int tx_ttl;
 };
 
@@ -53,9 +53,9 @@ static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
 	++priv->debug->tx_cache_miss;
 }
 
-static inline void cw1200_debug_tx_copy(struct cw1200_common *priv)
+static inline void cw1200_debug_tx_align(struct cw1200_common *priv)
 {
-	++priv->debug->tx_copy;
+	++priv->debug->tx_align;
 }
 
 static inline void cw1200_debug_tx_ttl(struct cw1200_common *priv)
@@ -99,7 +99,7 @@ static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
 {
 }
 
-static inline void cw1200_debug_tx_copy(struct cw1200_common *priv)
+static inline void cw1200_debug_tx_align(struct cw1200_common *priv)
 {
 }
 
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index ef5accb19a0..a03a17bb79c 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -508,14 +508,22 @@ cw1200_tx_h_crypt(struct cw1200_common *priv,
 
 static int
 cw1200_tx_h_align(struct cw1200_common *priv,
-		  struct cw1200_txinfo *t)
+		  struct cw1200_txinfo *t,
+		  u8 *flags)
 {
 	size_t offset = (size_t)t->skb->data & 3;
-	u8 *p;
 
 	if (!offset)
 		return 0;
 
+	if (offset & 1) {
+		wiphy_err(priv->hw->wiphy,
+			"Bug: attempt to transmit a frame "
+			"with wrong alignment: %d\n",
+			offset);
+		return -EINVAL;
+	}
+
 	if (skb_headroom(t->skb) < offset) {
 		wiphy_err(priv->hw->wiphy,
 			"Bug: no space allocated "
@@ -524,10 +532,11 @@ cw1200_tx_h_align(struct cw1200_common *priv,
 			skb_headroom(t->skb));
 		return -ENOMEM;
 	}
-	p = skb_push(t->skb, offset);
-	memmove(p, &p[offset], t->skb->len - offset);
-	skb_trim(t->skb, t->skb->len - offset);
-	cw1200_debug_tx_copy(priv);
+	skb_push(t->skb, offset);
+	t->hdrlen += offset;
+	t->txpriv.offset += offset;
+	*flags |= WSM_TX_2BYTES_SHIFT;
+	cw1200_debug_tx_align(priv);
 	return 0;
 }
 
@@ -645,7 +654,7 @@ cw1200_tx_h_rate_policy(struct cw1200_common *priv,
 	t->txpriv.rate_id = tx_policy_get(priv,
 		t->tx_info->control.rates, IEEE80211_TX_MAX_RATES,
 		&tx_policy_renew);
-	wsm->flags = t->txpriv.rate_id << 4;
+	wsm->flags |= t->txpriv.rate_id << 4;
 
 	if (tx_policy_renew) {
 		tx_policy_printk(KERN_DEBUG "[TX] TX policy renew.\n");
@@ -696,6 +705,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	};
 	struct wsm_tx *wsm;
 	bool tid_update = 0;
+	u8 flags = 0;
 	int ret;
 
 	t.rate = cw1200_get_tx_rate(priv,
@@ -722,7 +732,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	ret = cw1200_tx_h_crypt(priv, &t);
 	if (ret)
 		goto drop;
-	ret = cw1200_tx_h_align(priv, &t);
+	ret = cw1200_tx_h_align(priv, &t, &flags);
 	if (ret)
 		goto drop;
 	ret = cw1200_tx_h_action(priv, &t);
@@ -733,6 +743,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		ret = -ENOMEM;
 		goto drop;
 	}
+	wsm->flags |= flags;
 	cw1200_tx_h_bt(priv, &t, wsm);
 	cw1200_tx_h_rate_policy(priv, &t, wsm);
 
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 10303fae3c8..295b13f5676 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1313,12 +1313,12 @@ out:
 static bool wsm_handle_tx_data(struct cw1200_common *priv,
 			       const struct wsm_tx *wsm,
 			       const struct ieee80211_tx_info *tx_info,
-			       struct cw1200_queue *queue,
-			       int link_id)
+			       const struct cw1200_txpriv *txpriv,
+			       struct cw1200_queue *queue)
 {
 	bool handled = false;
 	const struct ieee80211_hdr *frame =
-		(struct ieee80211_hdr *)&wsm[1];
+		(struct ieee80211_hdr *) &((u8 *)wsm)[txpriv->offset];
 	__le16 fctl = frame->frame_control;
 	enum {
 		doProbe,
@@ -1349,7 +1349,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	case NL80211_IFTYPE_AP:
 		if (unlikely(!priv->join_status))
 			action = doDrop;
-		else if (unlikely(!(BIT(link_id) &
+		else if (unlikely(!(BIT(txpriv->raw_link_id) &
 				(BIT(0) | priv->link_id_map)))) {
 			wiphy_warn(priv->hw->wiphy,
 					"A frame with expired link id "
@@ -1396,7 +1396,6 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	switch (action) {
 	case doProbe:
 	{
-		const struct cw1200_txpriv *txpriv;
 		/* An interesting FW "feature". Device filters
 		 * probe responses.
 		 * The easiest way to get it back is to convert
@@ -1619,8 +1618,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 				continue;
 
 			if (wsm_handle_tx_data(priv, wsm,
-					tx_info, queue,
-					txpriv->raw_link_id))
+					tx_info, txpriv, queue))
 				continue;  /* Handled by WSM */
 
 			wsm->hdr.id &= __cpu_to_le16(
@@ -1635,7 +1633,8 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 
 			if (more) {
 				struct ieee80211_hdr *hdr =
-					(struct ieee80211_hdr *) &wsm[1];
+					(struct ieee80211_hdr *)
+					&((u8 *)wsm)[txpriv->offset];
 				/* more buffered multicast/broadcast frames
 				 *  ==> set MoreData flag in IEEE 802.11 header
 				 *  to inform PS STAs */
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index f0b07dc08a2..6698d771396 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -284,6 +284,9 @@ struct cw1200_common;
 /* Macro to fetch encryption key index. */
 #define WSM_RX_STATUS_KEY_IDX(status)	(((status >> 20)) & 0x0F)
 
+/* Frame Control field starts at Frame offset + 2 */
+#define WSM_TX_2BYTES_SHIFT		(BIT(7))
+
 /* Join mode */
 /* IBSS */
 #define WSM_JOIN_MODE_IBSS		(0)