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

Signed-off-by: Ajitpal.Singh <ajitpal.singh@stericsson.com>
Change-Id: I7bb45b01476a51052e5166feac5d05677bb56eed
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/35047
Reviewed-by: QATOOLS
Reviewed-by: QABUILD
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Tested-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>

5 files changed, 34 insertions, 19 deletions

diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index 926f7b99faa..50941846e28 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -289,8 +289,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 e6812dfc41e..96b11feab1c 100644
--- a/drivers/staging/cw1200/debug.h
+++ b/drivers/staging/cw1200/debug.h
@@ -25,7 +25,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;
 };
 
@@ -64,9 +64,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)
@@ -110,7 +110,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 a9177271d01..582b255a460 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -509,7 +509,8 @@ 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;
@@ -517,6 +518,14 @@ cw1200_tx_h_align(struct cw1200_common *priv,
 	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 "
@@ -526,9 +535,11 @@ cw1200_tx_h_align(struct cw1200_common *priv,
 		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);
+	t->hdr = (struct ieee80211_hdr *) p;
+	t->hdrlen += offset;
+	t->txpriv.offset += offset;
+	*flags |= WSM_TX_2BYTES_SHIFT;
+	cw1200_debug_tx_align(priv);
 	return 0;
 }
 
@@ -697,6 +708,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,
@@ -723,7 +735,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);
@@ -734,6 +746,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 bcda31364b1..a8c49494e49 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
@@ -1609,8 +1608,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(
@@ -1625,7 +1623,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 133de01cb96..bcccd790332 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)