From ad8ee13dd88d3658353a837cd42cf64ec37c2f8f Mon Sep 17 00:00:00 2001
From: Gertjan van Wingerde <gwingerde@gmail.com>
Date: Fri, 4 Dec 2009 23:47:03 +0100
Subject: [PATCH] --- yaml --- r: 183285 b: refs/heads/master c:
 2e331462fcb3d897921d8a0af4fca1b08c1b7269 h: refs/heads/master i:   183283:
 f80b92c1aacd01dbdd31e63ba459d4e4f4119f4d v: v3

---
 [refs]                                        |  2 +-
 .../drivers/net/wireless/rt2x00/rt2x00queue.c | 60 +++++++++----------
 2 files changed, 28 insertions(+), 34 deletions(-)

diff --git a/[refs] b/[refs]
index c12135042ce0..26abba65e73c 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 354e39dbb19f29ef28a9c2db9e55ff2a7435b35a
+refs/heads/master: 2e331462fcb3d897921d8a0af4fca1b08c1b7269
diff --git a/trunk/drivers/net/wireless/rt2x00/rt2x00queue.c b/trunk/drivers/net/wireless/rt2x00/rt2x00queue.c
index 719f4aebcaf6..7452fa850a0f 100644
--- a/trunk/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/trunk/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -177,45 +177,38 @@ void rt2x00queue_align_payload(struct sk_buff *skb, unsigned int header_length)
 
 void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length)
 {
-	unsigned int frame_length = skb->len;
+	unsigned int payload_length = skb->len - header_length;
 	unsigned int header_align = ALIGN_SIZE(skb, 0);
 	unsigned int payload_align = ALIGN_SIZE(skb, header_length);
 	unsigned int l2pad = L2PAD_SIZE(header_length);
 
-	if (header_align == payload_align) {
-		/*
-		 * Both header and payload must be moved the same
-		 * amount of bytes to align them properly. This means
-		 * we don't use the L2 padding but just move the entire
-		 * frame.
-		 */
-		rt2x00queue_align_frame(skb);
-	} else if (!payload_align) {
-		/*
-		 * Simple L2 padding, only the header needs to be moved,
-		 * the payload is already properly aligned.
-		 */
-		skb_push(skb, header_align);
-		memmove(skb->data, skb->data + header_align, header_length);
-	} else {
-		/*
-		 *
-		 * Complicated L2 padding, both header and payload need
-		 * to be moved. By default we only move to the start
-		 * of the buffer, so our header alignment needs to be
-		 * increased if there is not enough room for the header
-		 * to be moved.
-		 */
-		if (payload_align > header_align)
-			header_align += 4;
+	/*
+	 * Adjust the header alignment if the payload needs to be moved more
+	 * than the header.
+	 */
+	if (payload_align > header_align)
+		header_align += 4;
+
+	/* There is nothing to do if no alignment is needed */
+	if (!header_align)
+		return;
 
-		skb_push(skb, header_align);
-		memmove(skb->data, skb->data + header_align, header_length);
+	/* Reserve the amount of space needed in front of the frame */
+	skb_push(skb, header_align);
+
+	/*
+	 * Move the header.
+	 */
+	memmove(skb->data, skb->data + header_align, header_length);
+
+	/* Move the payload, if present and if required */
+	if (payload_length && payload_align)
 		memmove(skb->data + header_length + l2pad,
 			skb->data + header_length + l2pad + payload_align,
-			frame_length - header_length);
-		skb_trim(skb, frame_length + l2pad);
-	}
+			payload_length);
+
+	/* Trim the skb to the correct size */
+	skb_trim(skb, header_length + l2pad + payload_length);
 }
 
 void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length)
@@ -343,7 +336,8 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
 	 * Header and alignment information.
 	 */
 	txdesc->header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
-	if (test_bit(DRIVER_REQUIRE_L2PAD, &rt2x00dev->flags))
+	if (test_bit(DRIVER_REQUIRE_L2PAD, &rt2x00dev->flags) &&
+	    (entry->skb->len > txdesc->header_length))
 		txdesc->l2pad = L2PAD_SIZE(txdesc->header_length);
 
 	/*