4 files changed, 55 insertions, 25 deletions

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
index 12043f8be1bf..a659442b9c15 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
@@ -690,6 +690,7 @@ static int bcm43xx_wx_set_swencryption(struct net_device *net_dev,
         bcm->ieee->host_encrypt = !!on;
         bcm->ieee->host_decrypt = !!on;
         bcm->ieee->host_build_iv = !on;
+        bcm->ieee->host_strip_iv_icv = !on;
         spin_unlock_irqrestore(&bcm->irq_lock, flags);
         mutex_unlock(&bcm->mutex);
 
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_xmit.c b/drivers/net/wireless/bcm43xx/bcm43xx_xmit.c
index 0159e4e93201..a957bc861382 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_xmit.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_xmit.c
@@ -543,25 +543,6 @@ int bcm43xx_rx(struct bcm43xx_private *bcm,
                 break;
         }
 
-        frame_ctl = le16_to_cpu(wlhdr->frame_ctl);
-        if ((frame_ctl & IEEE80211_FCTL_PROTECTED) && !bcm->ieee->host_decrypt) {
-                frame_ctl &= ~IEEE80211_FCTL_PROTECTED;
-                wlhdr->frame_ctl = cpu_to_le16(frame_ctl);              
-                /* trim IV and ICV */
-                /* FIXME: this must be done only for WEP encrypted packets */
-                if (skb->len < 32) {
-                        dprintkl(KERN_ERR PFX "RX packet dropped (PROTECTED flag "
-                                              "set and length < 32)\n");
-                        return -EINVAL;
-                } else {                
-                        memmove(skb->data + 4, skb->data, 24);
-                        skb_pull(skb, 4);
-                        skb_trim(skb, skb->len - 4);
-                        stats.len -= 8;
-                }
-                wlhdr = (struct ieee80211_hdr_4addr *)(skb->data);
-        }
-        
         switch (WLAN_FC_GET_TYPE(frame_ctl)) {
         case IEEE80211_FTYPE_MGMT:
                 ieee80211_rx_mgt(bcm->ieee, wlhdr, &stats);
diff --git a/include/net/ieee80211.h b/include/net/ieee80211.h
index b174ebb277a9..cb255432e4e4 100644
--- a/include/net/ieee80211.h
+++ b/include/net/ieee80211.h
@@ -1037,6 +1037,10 @@ struct ieee80211_device {
         /* host performs multicast decryption */
         int host_mc_decrypt;
 
+        /* host should strip IV and ICV from protected frames */
+        /* meaningful only when hardware decryption is being used */
+        int host_strip_iv_icv;
+
         int host_open_frag;
         int host_build_iv;
         int ieee802_1x;         /* is IEEE 802.1X used */
diff --git a/net/ieee80211/ieee80211_rx.c b/net/ieee80211/ieee80211_rx.c
index 2759312a4204..d9265195656d 100644
--- a/net/ieee80211/ieee80211_rx.c
+++ b/net/ieee80211/ieee80211_rx.c
@@ -415,17 +415,16 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
             ieee->host_mc_decrypt : ieee->host_decrypt;
 
         if (can_be_decrypted) {
-                int idx = 0;
                 if (skb->len >= hdrlen + 3) {
                         /* Top two-bits of byte 3 are the key index */
-                        idx = skb->data[hdrlen + 3] >> 6;
+                        keyidx = skb->data[hdrlen + 3] >> 6;
                 }
 
-                /* ieee->crypt[] is WEP_KEY (4) in length.  Given that idx
-                 * is only allowed 2-bits of storage, no value of idx can
-                 * be provided via above code that would result in idx
+                /* ieee->crypt[] is WEP_KEY (4) in length.  Given that keyidx
+                 * is only allowed 2-bits of storage, no value of keyidx can
+                 * be provided via above code that would result in keyidx
                  * being out of range */
-                crypt = ieee->crypt[idx];
+                crypt = ieee->crypt[keyidx];
 
 #ifdef NOT_YET
                 sta = NULL;
@@ -655,6 +654,51 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
                 goto rx_dropped;
         }
 
+        /* If the frame was decrypted in hardware, we may need to strip off
+         * any security data (IV, ICV, etc) that was left behind */
+        if (!can_be_decrypted && (fc & IEEE80211_FCTL_PROTECTED) &&
+            ieee->host_strip_iv_icv) {
+                int trimlen = 0;
+
+                /* Top two-bits of byte 3 are the key index */
+                if (skb->len >= hdrlen + 3)
+                        keyidx = skb->data[hdrlen + 3] >> 6;
+
+                /* To strip off any security data which appears before the
+                 * payload, we simply increase hdrlen (as the header gets
+                 * chopped off immediately below). For the security data which
+                 * appears after the payload, we use skb_trim. */
+
+                switch (ieee->sec.encode_alg[keyidx]) {
+                case SEC_ALG_WEP:
+                        /* 4 byte IV */
+                        hdrlen += 4;
+                        /* 4 byte ICV */
+                        trimlen = 4;
+                        break;
+                case SEC_ALG_TKIP:
+                        /* 4 byte IV, 4 byte ExtIV */
+                        hdrlen += 8;
+                        /* 8 byte MIC, 4 byte ICV */
+                        trimlen = 12;
+                        break;
+                case SEC_ALG_CCMP:
+                        /* 8 byte CCMP header */
+                        hdrlen += 8;
+                        /* 8 byte MIC */
+                        trimlen = 8;
+                        break;
+                }
+
+                if (skb->len < trimlen)
+                        goto rx_dropped;
+
+                __skb_trim(skb, skb->len - trimlen);
+
+                if (skb->len < hdrlen)
+                        goto rx_dropped;
+        }
+
         /* skb: hdr + (possible reassembled) full plaintext payload */
 
         payload = skb->data + hdrlen;