1 files changed, 15 insertions, 14 deletions

diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 6b12d81ba94b..c1cb20fceaff 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -728,35 +728,36 @@ static inline void mwl8k_remove_dma_header(struct sk_buff *skb, __le16 qos)
 static inline void mwl8k_add_dma_header(struct sk_buff *skb)
 {
         struct ieee80211_hdr *wh;
-        u32 hdrlen, pktlen;
+        int hdrlen;
         struct mwl8k_dma_data *tr;
 
+        /*
+         * Add a firmware DMA header; the firmware requires that we
+         * present a 2-byte payload length followed by a 4-address
+         * header (without QoS field), followed (optionally) by any
+         * WEP/ExtIV header (but only filled in for CCMP).
+         */
         wh = (struct ieee80211_hdr *)skb->data;
+
         hdrlen = ieee80211_hdrlen(wh->frame_control);
-        pktlen = skb->len;
+        if (hdrlen != sizeof(*tr))
+                skb_push(skb, sizeof(*tr) - hdrlen);
 
-        /*
-         * Copy up/down the 802.11 header; the firmware requires
-         * we present a 2-byte payload length followed by a
-         * 4-address header (w/o QoS), followed (optionally) by
-         * any WEP/ExtIV header (but only filled in for CCMP).
-         */
-        if (hdrlen != sizeof(struct mwl8k_dma_data))
-                skb_push(skb, sizeof(struct mwl8k_dma_data) - hdrlen);
+        if (ieee80211_is_data_qos(wh->frame_control))
+                hdrlen -= 2;
 
         tr = (struct mwl8k_dma_data *)skb->data;
         if (wh != &tr->wh)
                 memmove(&tr->wh, wh, hdrlen);
-
-        /* Clear addr4 */
-        memset(tr->wh.addr4, 0, ETH_ALEN);
+        if (hdrlen != sizeof(tr->wh))
+                memset(((void *)&tr->wh) + hdrlen, 0, sizeof(tr->wh) - hdrlen);
 
         /*
          * Firmware length is the length of the fully formed "802.11
          * payload".  That is, everything except for the 802.11 header.
          * This includes all crypto material including the MIC.
          */
-        tr->fwlen = cpu_to_le16(pktlen - hdrlen);
+        tr->fwlen = cpu_to_le16(skb->len - sizeof(*tr));
 }