1 files changed, 42 insertions, 6 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index a5664bd8493e..44e5b3279ca7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -148,6 +148,35 @@ void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
         dev_kfree_skb_any(skb);
 }
 
+void rt2x00queue_payload_align(struct sk_buff *skb,
+                               bool l2pad, unsigned int header_length)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        unsigned int frame_length = skb->len;
+        unsigned int align = ALIGN_SIZE(skb, header_length);
+
+        if (!align)
+                return;
+
+        if (l2pad) {
+                if (skbdesc->flags & SKBDESC_L2_PADDED) {
+                        /* Remove L2 padding */
+                        memmove(skb->data + align, skb->data, header_length);
+                        skb_pull(skb, align);
+                        skbdesc->flags &= ~SKBDESC_L2_PADDED;
+                } else {
+                        /* Add L2 padding */
+                        skb_push(skb, align);
+                        memmove(skb->data, skb->data + align, header_length);
+                        skbdesc->flags |= SKBDESC_L2_PADDED;
+                }
+        } else {
+                /* Generic payload alignment to 4-byte boundary */
+                skb_push(skb, align);
+                memmove(skb->data, skb->data + align, frame_length);
+        }
+}
+
 static void rt2x00queue_create_tx_descriptor_seq(struct queue_entry *entry,
                                                  struct txentry_desc *txdesc)
 {
@@ -259,6 +288,12 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
         txdesc->aifs = entry->queue->aifs;
 
         /*
+         * Header and alignment information.
+         */
+        txdesc->header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
+        txdesc->l2pad = ALIGN_SIZE(entry->skb, txdesc->header_length);
+
+        /*
          * Check whether this frame is to be acked.
          */
         if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
@@ -326,6 +361,7 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
          * Apply TX descriptor handling by components
          */
         rt2x00crypto_create_tx_descriptor(entry, txdesc);
+        rt2x00ht_create_tx_descriptor(entry, txdesc, hwrate);
         rt2x00queue_create_tx_descriptor_seq(entry, txdesc);
         rt2x00queue_create_tx_descriptor_plcp(entry, txdesc, hwrate);
 }
@@ -368,7 +404,6 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
         struct txentry_desc txdesc;
         struct skb_frame_desc *skbdesc;
-        unsigned int iv_len = 0;
         u8 rate_idx, rate_flags;
 
         if (unlikely(rt2x00queue_full(queue)))
@@ -390,9 +425,6 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         entry->skb = skb;
         rt2x00queue_create_tx_descriptor(entry, &txdesc);
 
-        if (IEEE80211_SKB_CB(skb)->control.hw_key != NULL)
-                iv_len = IEEE80211_SKB_CB(skb)->control.hw_key->iv_len;
-
         /*
          * All information is retrieved from the skb->cb array,
          * now we should claim ownership of the driver part of that
@@ -415,11 +447,15 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
             !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags)) {
                 if (test_bit(DRIVER_REQUIRE_COPY_IV, &queue->rt2x00dev->flags))
-                        rt2x00crypto_tx_copy_iv(skb, iv_len);
+                        rt2x00crypto_tx_copy_iv(skb, &txdesc);
                 else
-                        rt2x00crypto_tx_remove_iv(skb, iv_len);
+                        rt2x00crypto_tx_remove_iv(skb, &txdesc);
         }
 
+        if (test_bit(DRIVER_REQUIRE_L2PAD, &queue->rt2x00dev->flags))
+                rt2x00queue_payload_align(entry->skb, true,
+                                          txdesc.header_length);
+
         /*
          * It could be possible that the queue was corrupted and this
          * call failed. Since we always return NETDEV_TX_OK to mac80211,