rt2x00: Add support for L2 padding during TX/RX

Some hardware require L2 padding between header and payload
because both must be aligned to a 4-byte boundary. This hardware
also is easier during the RX path since we no longer need to
move the entire payload but rather only the header to remove
the padding (mac80211 only wants the payload to be 4-byte aligned).

Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 39 insertions, 0 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 6f78915b364c..bc1742c1d51c 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))
@@ -416,6 +451,10 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
                         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,