[rt2x00-users] [PATCH 04/12] rt2x00: Fix TX/RX padding for rt2800pci

[Benoit PAPILLAULT]

This change has been tested with a monitor interface sending/receiving every
possible frames (be it a valid 802.11 frame or not). It appears that padding
is required only for QoS data or DS to DS frames.

Signed-off-by: Benoit PAPILLAULT <benoit.papillault at free.fr>
---
 drivers/net/wireless/rt2x00/rt2800pci.c |    6 ++++--
 drivers/net/wireless/rt2x00/rt2x00.h    |    2 +-
 drivers/net/wireless/rt2x00/rt2x00dev.c |    7 ++++---
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index f018cda..d934f13 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -2297,6 +2297,7 @@ static void rt2800pci_fill_rxdone(struct queue_entry *entry,
 	u32 rxwi1;
 	u32 rxwi2;
 	u32 rxwi3;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 
 	rt2x00_desc_read(rxd, 3, &rxd3);
 	rt2x00_desc_read(rxwi, 0, &rxwi0);
@@ -2336,8 +2337,10 @@ static void rt2800pci_fill_rxdone(struct queue_entry *entry,
 	if (rt2x00_get_field32(rxd3, RXD_W3_MY_BSS))
 		rxdesc->dev_flags |= RXDONE_MY_BSS;
 
-	if (rt2x00_get_field32(rxd3, RXD_W3_L2PAD))
+	if (rt2x00_get_field32(rxd3, RXD_W3_L2PAD)) {
 		rxdesc->dev_flags |= RXDONE_L2PAD;
+		skbdesc->flags |= SKBDESC_L2_PADDED;
+	}
 
 	if (rt2x00_get_field32(rxwi1, RXWI_W1_SHORT_GI))
 		rxdesc->flags |= RX_FLAG_SHORT_GI;
@@ -2378,7 +2381,6 @@ static void rt2800pci_fill_rxdone(struct queue_entry *entry,
 	 * Remove TXWI descriptor from start of buffer.
 	 */
 	skb_pull(entry->skb, RXWI_DESC_SIZE);
-	skb_trim(entry->skb, rxdesc->size);
 }
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index de3f13a..61476ba 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -109,7 +109,7 @@
  * amount of bytes needed to move the data.
  */
 #define ALIGN_SIZE(__skb, __header) \
-	(  ((unsigned long)((__skb)->data + (__header))) & 3 )
+	( ((__header) < 24) ? 0 : ((__header) & 3) )
 
 /*
  * Standard timing and size defines.
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 4fff3a8..d8192b1 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -393,9 +393,6 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
 	memset(&rxdesc, 0, sizeof(rxdesc));
 	rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
 
-	/* Trim buffer to correct size */
-	skb_trim(entry->skb, rxdesc.size);
-
 	/*
 	 * The data behind the ieee80211 header must be
 	 * aligned on a 4 byte boundary.
@@ -403,6 +400,10 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
 	header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
 	l2pad = !!(rxdesc.dev_flags & RXDONE_L2PAD);
 
+	/* Trim buffer to correct size (it still contains the padding) */
+	skb_trim(entry->skb, rxdesc.size
+		 + (l2pad ? ALIGN_SIZE(entry->skb, header_length) : 0));
+
 	/*
 	 * Hardware might have stripped the IV/EIV/ICV data,
 	 * in that case it is possible that the data was
-- 
1.6.2.4