wl1271: Correct TKIP header space handling in TX path

Correct the position to which TKIP header space is appended for TX
packets.

Signed-off-by: Juuso Oikarinen <juuso.oikarinen@nokia.com>
Reviewed-by: Vidhya Govindan <vidhya.govindan@nokia.com>
Signed-off-by: Luciano Coelho <luciano.coelho@nokia.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/wl12xx/wl1271_main.c

@@ -1160,12 +1160,9 @@ static int wl1271_register_hw(struct wl1271 *wl)
 
 static int wl1271_init_ieee80211(struct wl1271 *wl)
 {
-	/*
-	 * The tx descriptor buffer and the TKIP space.
-	 *
-	 * FIXME: add correct 1271 descriptor size
-	 */
-	wl->hw->extra_tx_headroom = WL1271_TKIP_IV_SPACE;
+	/* The tx descriptor buffer and the TKIP space. */
+	wl->hw->extra_tx_headroom = WL1271_TKIP_IV_SPACE +
+		sizeof(struct wl1271_tx_hw_descr);
 
 	/* unit us */
 	/* FIXME: find a proper value */

drivers/net/wireless/wl12xx/wl1271_tx.c

@@ -92,6 +92,14 @@ static int wl1271_tx_fill_hdr(struct wl1271 *wl, struct sk_buff *skb,
 
 	desc = (struct wl1271_tx_hw_descr *) skb->data;
 
+	/* relocate space for security header */
+	if (extra) {
+		void *framestart = skb->data + sizeof(*desc);
+		u16 fc = *(u16 *)(framestart + extra);
+		int hdrlen = ieee80211_hdrlen(fc);
+		memmove(framestart, framestart + extra, hdrlen);
+	}
+
 	/* configure packet life time */
 	desc->start_time = jiffies_to_usecs(jiffies) - wl->time_offset;
 	desc->life_time = TX_HW_MGMT_PKT_LIFETIME_TU;
@@ -257,7 +265,6 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 
 	struct ieee80211_tx_info *info;
 	struct sk_buff *skb;
-	u32 header_len;
 	u16 seq;
 	int id = result->id;
 
@@ -295,22 +302,22 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 		wl->tx_security_seq_32++;
 	wl->tx_security_seq_16 = seq;
 
-	/* get header len */
+	/* remove private header from packet */
+	skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
+
+	/* remove TKIP header space if present */
 	if (info->control.hw_key &&
-	    info->control.hw_key->alg == ALG_TKIP)
-		header_len = WL1271_TKIP_IV_SPACE +
-			sizeof(struct wl1271_tx_hw_descr);
-	else
-		header_len = sizeof(struct wl1271_tx_hw_descr);
+	    info->control.hw_key->alg == ALG_TKIP) {
+		int hdrlen = ieee80211_get_hdrlen_from_skb(skb);
+		memmove(skb->data + WL1271_TKIP_IV_SPACE, skb->data, hdrlen);
+		skb_pull(skb, WL1271_TKIP_IV_SPACE);
+	}
 
 	wl1271_debug(DEBUG_TX, "tx status id %u skb 0x%p failures %u rate 0x%x"
 		     " status 0x%x",
 		     result->id, skb, result->ack_failures,
 		     result->rate_class_index, result->status);
 
-	/* remove private header from packet */
-	skb_pull(skb, header_len);
-
 	/* return the packet to the stack */
 	ieee80211_tx_status(wl->hw, skb);
 	wl->tx_frames[result->id] = NULL;