rt2x00: Put 802.11 data on 4 byte boundary

Check the size of the ieee80211 header during rxdone
and make sure the data behind the ieee80211 header
is placed on a 4 byte boundary.

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

diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 2780df0..6d5d9ab 100644 (file)
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -124,7 +124,10 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
        struct data_entry *entry;
        struct data_desc *rxd;
        struct sk_buff *skb;
+       struct ieee80211_hdr *hdr;
        struct rxdata_entry_desc desc;
+       int header_size;
+       int align;
        u32 word;
 
        while (1) {
@@ -138,17 +141,26 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
                memset(&desc, 0x00, sizeof(desc));
                rt2x00dev->ops->lib->fill_rxdone(entry, &desc);
 
+               hdr = (struct ieee80211_hdr *)entry->data_addr;
+               header_size =
+                   ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
+
+               /*
+                * The data behind the ieee80211 header must be
+                * aligned on a 4 byte boundary.
+                */
+               align = NET_IP_ALIGN + (2 * (header_size % 4 == 0));
+
                /*
                 * Allocate the sk_buffer, initialize it and copy
                 * all data into it.
                 */
-               skb = dev_alloc_skb(desc.size + NET_IP_ALIGN);
+               skb = dev_alloc_skb(desc.size + align);
                if (!skb)
                        return;
 
-               skb_reserve(skb, NET_IP_ALIGN);
-               skb_put(skb, desc.size);
-               memcpy(skb->data, entry->data_addr, desc.size);
+               skb_reserve(skb, align);
+               memcpy(skb_put(skb, desc.size), entry->data_addr, desc.size);
 
                /*
                 * Send the frame to rt2x00lib for further processing.

diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 1f5675d..ab4797e 100644 (file)
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -221,7 +221,9 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
        struct data_ring *ring = entry->ring;
        struct rt2x00_dev *rt2x00dev = ring->rt2x00dev;
        struct sk_buff *skb;
+       struct ieee80211_hdr *hdr;
        struct rxdata_entry_desc desc;
+       int header_size;
        int frame_size;
 
        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
@@ -253,9 +255,20 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
        skb_put(skb, frame_size);
 
        /*
-        * Trim the skb_buffer to only contain the valid
-        * frame data (so ignore the device's descriptor).
+        * The data behind the ieee80211 header must be
+        * aligned on a 4 byte boundary.
+        * After that trim the entire buffer down to only
+        * contain the valid frame data excluding the device
+        * descriptor.
         */
+       hdr = (struct ieee80211_hdr *)entry->skb->data;
+       header_size =
+           ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
+
+       if (header_size % 4 == 0) {
+               skb_push(entry->skb, 2);
+               memmove(entry->skb->data, entry->skb->data + 2, skb->len - 2);
+       }
        skb_trim(entry->skb, desc.size);
 
        /*