1 files changed, 12 insertions, 38 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 8e68f87ab13c..66ff36447b94 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -158,55 +158,29 @@ void rt2x00queue_align_frame(struct sk_buff *skb)
         skb_trim(skb, frame_length);
 }
 
-void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length)
+/*
+ * H/W needs L2 padding between the header and the paylod if header size
+ * is not 4 bytes aligned.
+ */
+void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int hdr_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 = payload_length ? L2PAD_SIZE(header_length) : 0;
+        unsigned int l2pad = (skb->len > hdr_len) ? L2PAD_SIZE(hdr_len) : 0;
 
-        /*
-         * 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)
+        if (!l2pad)
                 return;
 
-        /* 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,
-                        payload_length);
-
-        /* Trim the skb to the correct size */
-        skb_trim(skb, header_length + l2pad + payload_length);
+        skb_push(skb, l2pad);
+        memmove(skb->data, skb->data + l2pad, hdr_len);
 }
 
-void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length)
+void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int hdr_len)
 {
-        /*
-         * L2 padding is only present if the skb contains more than just the
-         * IEEE 802.11 header.
-         */
-        unsigned int l2pad = (skb->len > header_length) ?
-                                L2PAD_SIZE(header_length) : 0;
+        unsigned int l2pad = (skb->len > hdr_len) ? L2PAD_SIZE(hdr_len) : 0;
 
         if (!l2pad)
                 return;
 
-        memmove(skb->data + l2pad, skb->data, header_length);
+        memmove(skb->data + l2pad, skb->data, hdr_len);
         skb_pull(skb, l2pad);
 }