2 files changed, 37 insertions, 28 deletions

diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 12d0717c399..34561e6e816 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -451,8 +451,8 @@ static int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
                         }
                         if (err)
                                 goto err;
-
-                }
+                        }
+                        break;
                 case PDR_PRISM_ZIF_TX_IQ_CALIBRATION:
                         priv->iq_autocal = kmalloc(data_len, GFP_KERNEL);
                         if (!priv->iq_autocal) {
@@ -745,7 +745,7 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
                 struct p54_hdr *entry_hdr;
                 struct p54_tx_data *entry_data;
-                int pad = 0;
+                unsigned int pad = 0, frame_len;
 
                 range = (void *)info->rate_driver_data;
                 if (range->start_addr != addr) {
@@ -768,6 +768,7 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
                 __skb_unlink(entry, &priv->tx_queue);
                 spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
 
+                frame_len = entry->len;
                 entry_hdr = (struct p54_hdr *) entry->data;
                 entry_data = (struct p54_tx_data *) entry_hdr->data;
                 priv->tx_stats[entry_data->hw_queue].len--;
@@ -814,15 +815,28 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
                 info->status.ack_signal = p54_rssi_to_dbm(dev,
                                 (int)payload->ack_rssi);
 
-                if (entry_data->key_type == P54_CRYPTO_TKIPMICHAEL) {
+                /* Undo all changes to the frame. */
+                switch (entry_data->key_type) {
+                case P54_CRYPTO_TKIPMICHAEL: {
                         u8 *iv = (u8 *)(entry_data->align + pad +
-                                        entry_data->crypt_offset);
+                                        entry_data->crypt_offset);
 
                         /* Restore the original TKIP IV. */
                         iv[2] = iv[0];
                         iv[0] = iv[1];
                         iv[1] = (iv[0] | 0x20) & 0x7f;  /* WEPSeed - 8.3.2.2 */
+
+                        frame_len -= 12; /* remove TKIP_MMIC + TKIP_ICV */
+                        break;
+                        }
+                case P54_CRYPTO_AESCCMP:
+                        frame_len -= 8; /* remove CCMP_MIC */
+                        break;
+                case P54_CRYPTO_WEP:
+                        frame_len -= 4; /* remove WEP_ICV */
+                        break;
                 }
+                skb_trim(entry, frame_len);
                 skb_pull(entry, sizeof(*hdr) + pad + sizeof(*entry_data));
                 ieee80211_tx_status_irqsafe(dev, entry);
                 goto out;
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index 6a6a72f6f82..5de2ebfb28c 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -144,11 +144,8 @@ static void p54u_tx_cb(struct urb *urb)
         struct sk_buff *skb = urb->context;
         struct ieee80211_hw *dev = (struct ieee80211_hw *)
                 usb_get_intfdata(usb_ifnum_to_if(urb->dev, 0));
-        struct p54u_priv *priv = dev->priv;
 
-        skb_pull(skb, priv->common.tx_hdr_len);
-        if (FREE_AFTER_TX(skb))
-                p54_free_skb(dev, skb);
+        p54_free_skb(dev, skb);
 }
 
 static void p54u_tx_dummy_cb(struct urb *urb) { }
@@ -230,7 +227,10 @@ static void p54u_tx_3887(struct ieee80211_hw *dev, struct sk_buff *skb)
                           p54u_tx_dummy_cb, dev);
         usb_fill_bulk_urb(data_urb, priv->udev,
                           usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA),
-                          skb->data, skb->len, p54u_tx_cb, skb);
+                          skb->data, skb->len, FREE_AFTER_TX(skb) ?
+                          p54u_tx_cb : p54u_tx_dummy_cb, skb);
+        addr_urb->transfer_flags |= URB_ZERO_PACKET;
+        data_urb->transfer_flags |= URB_ZERO_PACKET;
 
         usb_anchor_urb(addr_urb, &priv->submitted);
         err = usb_submit_urb(addr_urb, GFP_ATOMIC);
@@ -239,7 +239,7 @@ static void p54u_tx_3887(struct ieee80211_hw *dev, struct sk_buff *skb)
                 goto out;
         }
 
-        usb_anchor_urb(addr_urb, &priv->submitted);
+        usb_anchor_urb(data_urb, &priv->submitted);
         err = usb_submit_urb(data_urb, GFP_ATOMIC);
         if (err)
                 usb_unanchor_urb(data_urb);
@@ -269,28 +269,24 @@ static void p54u_tx_lm87(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
         struct p54u_priv *priv = dev->priv;
         struct urb *data_urb;
-        struct lm87_tx_hdr *hdr;
-        __le32 checksum;
-        __le32 addr = ((struct p54_hdr *)skb->data)->req_id;
+        struct lm87_tx_hdr *hdr = (void *)skb->data - sizeof(*hdr);
 
         data_urb = usb_alloc_urb(0, GFP_ATOMIC);
         if (!data_urb)
                 return;
 
-        checksum = p54u_lm87_chksum((__le32 *)skb->data, skb->len);
-        hdr = (struct lm87_tx_hdr *)skb_push(skb, sizeof(*hdr));
-        hdr->chksum = checksum;
-        hdr->device_addr = addr;
+        hdr->chksum = p54u_lm87_chksum((__le32 *)skb->data, skb->len);
+        hdr->device_addr = ((struct p54_hdr *)skb->data)->req_id;
 
         usb_fill_bulk_urb(data_urb, priv->udev,
                           usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA),
-                          skb->data, skb->len, p54u_tx_cb, skb);
+                          hdr, skb->len + sizeof(*hdr),  FREE_AFTER_TX(skb) ?
+                          p54u_tx_cb : p54u_tx_dummy_cb, skb);
         data_urb->transfer_flags |= URB_ZERO_PACKET;
 
         usb_anchor_urb(data_urb, &priv->submitted);
         if (usb_submit_urb(data_urb, GFP_ATOMIC)) {
                 usb_unanchor_urb(data_urb);
-                skb_pull(skb, sizeof(*hdr));
                 p54_free_skb(dev, skb);
         }
         usb_free_urb(data_urb);
@@ -300,11 +296,9 @@ static void p54u_tx_net2280(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
         struct p54u_priv *priv = dev->priv;
         struct urb *int_urb, *data_urb;
-        struct net2280_tx_hdr *hdr;
+        struct net2280_tx_hdr *hdr = (void *)skb->data - sizeof(*hdr);
         struct net2280_reg_write *reg;
         int err = 0;
-        __le32 addr = ((struct p54_hdr *) skb->data)->req_id;
-        __le16 len = cpu_to_le16(skb->len);
 
         reg = kmalloc(sizeof(*reg), GFP_ATOMIC);
         if (!reg)
@@ -327,10 +321,9 @@ static void p54u_tx_net2280(struct ieee80211_hw *dev, struct sk_buff *skb)
         reg->addr = cpu_to_le32(P54U_DEV_BASE);
         reg->val = cpu_to_le32(ISL38XX_DEV_INT_DATA);
 
-        hdr = (void *)skb_push(skb, sizeof(*hdr));
         memset(hdr, 0, sizeof(*hdr));
-        hdr->len = len;
-        hdr->device_addr = addr;
+        hdr->len = cpu_to_le16(skb->len);
+        hdr->device_addr = ((struct p54_hdr *) skb->data)->req_id;
 
         usb_fill_bulk_urb(int_urb, priv->udev,
                 usb_sndbulkpipe(priv->udev, P54U_PIPE_DEV), reg, sizeof(*reg),
@@ -341,11 +334,13 @@ static void p54u_tx_net2280(struct ieee80211_hw *dev, struct sk_buff *skb)
          * free what's inside the transfer_buffer after the callback routine
          * has completed.
          */
-        int_urb->transfer_flags |= URB_FREE_BUFFER;
+        int_urb->transfer_flags |= URB_FREE_BUFFER | URB_ZERO_PACKET;
 
         usb_fill_bulk_urb(data_urb, priv->udev,
                           usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA),
-                          skb->data, skb->len, p54u_tx_cb, skb);
+                          hdr, skb->len + sizeof(*hdr), FREE_AFTER_TX(skb) ?
+                          p54u_tx_cb : p54u_tx_dummy_cb, skb);
+        data_urb->transfer_flags |= URB_ZERO_PACKET;
 
         usb_anchor_urb(int_urb, &priv->submitted);
         err = usb_submit_urb(int_urb, GFP_ATOMIC);