2 files changed, 117 insertions, 88 deletions

diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index dbfd684e3e2e..a182e423109b 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1541,16 +1541,10 @@ static ieee80211_rx_result debug_noinline
 ieee80211_rx_h_amsdu(struct ieee80211_rx_data *rx)
 {
         struct net_device *dev = rx->sdata->dev;
-        struct ieee80211_local *local = rx->local;
-        u16 ethertype;
-        u8 *payload;
-        struct sk_buff *skb = rx->skb, *frame = NULL;
+        struct sk_buff *skb = rx->skb;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         __le16 fc = hdr->frame_control;
-        const struct ethhdr *eth;
-        int remaining, err;
-        u8 dst[ETH_ALEN];
-        u8 src[ETH_ALEN];
+        struct sk_buff_head frame_list;
 
         if (unlikely(!ieee80211_is_data(fc)))
                 return RX_CONTINUE;
@@ -1561,94 +1555,34 @@ ieee80211_rx_h_amsdu(struct ieee80211_rx_data *rx)
         if (!(rx->flags & IEEE80211_RX_AMSDU))
                 return RX_CONTINUE;
 
-        err = __ieee80211_data_to_8023(rx);
-        if (unlikely(err))
+        if (ieee80211_has_a4(hdr->frame_control) &&
+            rx->sdata->vif.type == NL80211_IFTYPE_AP_VLAN &&
+            !rx->sdata->u.vlan.sta)
                 return RX_DROP_UNUSABLE;
 
-        skb->dev = dev;
-
-        dev->stats.rx_packets++;
-        dev->stats.rx_bytes += skb->len;
-
-        /* skip the wrapping header */
-        eth = (struct ethhdr *) skb_pull(skb, sizeof(struct ethhdr));
-        if (!eth)
+        if (is_multicast_ether_addr(hdr->addr1) &&
+            ((rx->sdata->vif.type == NL80211_IFTYPE_AP_VLAN &&
+              rx->sdata->u.vlan.sta) ||
+             (rx->sdata->vif.type == NL80211_IFTYPE_STATION &&
+              rx->sdata->u.mgd.use_4addr)))
                 return RX_DROP_UNUSABLE;
 
-        while (skb != frame) {
-                u8 padding;
-                __be16 len = eth->h_proto;
-                unsigned int subframe_len = sizeof(struct ethhdr) + ntohs(len);
-
-                remaining = skb->len;
-                memcpy(dst, eth->h_dest, ETH_ALEN);
-                memcpy(src, eth->h_source, ETH_ALEN);
+        skb->dev = dev;
+        __skb_queue_head_init(&frame_list);
 
-                padding = ((4 - subframe_len) & 0x3);
-                /* the last MSDU has no padding */
-                if (subframe_len > remaining)
-                        return RX_DROP_UNUSABLE;
+        ieee80211_amsdu_to_8023s(skb, &frame_list, dev->dev_addr,
+                                 rx->sdata->vif.type,
+                                 rx->local->hw.extra_tx_headroom);
 
-                skb_pull(skb, sizeof(struct ethhdr));
-                /* if last subframe reuse skb */
-                if (remaining <= subframe_len + padding)
-                        frame = skb;
-                else {
-                        /*
-                         * Allocate and reserve two bytes more for payload
-                         * alignment since sizeof(struct ethhdr) is 14.
-                         */
-                        frame = dev_alloc_skb(
-                                ALIGN(local->hw.extra_tx_headroom, 4) +
-                                subframe_len + 2);
-
-                        if (frame == NULL)
-                                return RX_DROP_UNUSABLE;
-
-                        skb_reserve(frame,
-                                    ALIGN(local->hw.extra_tx_headroom, 4) +
-                                    sizeof(struct ethhdr) + 2);
-                        memcpy(skb_put(frame, ntohs(len)), skb->data,
-                                ntohs(len));
-
-                        eth = (struct ethhdr *) skb_pull(skb, ntohs(len) +
-                                                        padding);
-                        if (!eth) {
-                                dev_kfree_skb(frame);
-                                return RX_DROP_UNUSABLE;
-                        }
-                }
-
-                skb_reset_network_header(frame);
-                frame->dev = dev;
-                frame->priority = skb->priority;
-                rx->skb = frame;
-
-                payload = frame->data;
-                ethertype = (payload[6] << 8) | payload[7];
-
-                if (likely((compare_ether_addr(payload, rfc1042_header) == 0 &&
-                            ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
-                           compare_ether_addr(payload,
-                                              bridge_tunnel_header) == 0)) {
-                        /* remove RFC1042 or Bridge-Tunnel
-                         * encapsulation and replace EtherType */
-                        skb_pull(frame, 6);
-                        memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
-                        memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
-                } else {
-                        memcpy(skb_push(frame, sizeof(__be16)),
-                               &len, sizeof(__be16));
-                        memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
-                        memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
-                }
+        while (!skb_queue_empty(&frame_list)) {
+                rx->skb = __skb_dequeue(&frame_list);
 
                 if (!ieee80211_frame_allowed(rx, fc)) {
-                        if (skb == frame) /* last frame */
-                                return RX_DROP_UNUSABLE;
-                        dev_kfree_skb(frame);
+                        dev_kfree_skb(rx->skb);
                         continue;
                 }
+                dev->stats.rx_packets++;
+                dev->stats.rx_bytes += rx->skb->len;
 
                 ieee80211_deliver_skb(rx);
         }
diff --git a/net/wireless/util.c b/net/wireless/util.c
index a3c841a255db..23557c1d0a9c 100644
--- a/net/wireless/util.c
+++ b/net/wireless/util.c
@@ -285,7 +285,7 @@ static int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
         }
 }
 
-int ieee80211_data_to_8023(struct sk_buff *skb, u8 *addr,
+int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                            enum nl80211_iftype iftype)
 {
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
@@ -383,7 +383,7 @@ int ieee80211_data_to_8023(struct sk_buff *skb, u8 *addr,
 }
 EXPORT_SYMBOL(ieee80211_data_to_8023);
 
-int ieee80211_data_from_8023(struct sk_buff *skb, u8 *addr,
+int ieee80211_data_from_8023(struct sk_buff *skb, const u8 *addr,
                              enum nl80211_iftype iftype, u8 *bssid, bool qos)
 {
         struct ieee80211_hdr hdr;
@@ -497,6 +497,101 @@ int ieee80211_data_from_8023(struct sk_buff *skb, u8 *addr,
 }
 EXPORT_SYMBOL(ieee80211_data_from_8023);
 
+
+void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
+                              const u8 *addr, enum nl80211_iftype iftype,
+                              const unsigned int extra_headroom)
+{
+        struct sk_buff *frame = NULL;
+        u16 ethertype;
+        u8 *payload;
+        const struct ethhdr *eth;
+        int remaining, err;
+        u8 dst[ETH_ALEN], src[ETH_ALEN];
+
+        err = ieee80211_data_to_8023(skb, addr, iftype);
+        if (err)
+                goto out;
+
+        /* skip the wrapping header */
+        eth = (struct ethhdr *) skb_pull(skb, sizeof(struct ethhdr));
+        if (!eth)
+                goto out;
+
+        while (skb != frame) {
+                u8 padding;
+                __be16 len = eth->h_proto;
+                unsigned int subframe_len = sizeof(struct ethhdr) + ntohs(len);
+
+                remaining = skb->len;
+                memcpy(dst, eth->h_dest, ETH_ALEN);
+                memcpy(src, eth->h_source, ETH_ALEN);
+
+                padding = (4 - subframe_len) & 0x3;
+                /* the last MSDU has no padding */
+                if (subframe_len > remaining)
+                        goto purge;
+
+                skb_pull(skb, sizeof(struct ethhdr));
+                /* reuse skb for the last subframe */
+                if (remaining <= subframe_len + padding)
+                        frame = skb;
+                else {
+                        unsigned int hlen = ALIGN(extra_headroom, 4);
+                        /*
+                         * Allocate and reserve two bytes more for payload
+                         * alignment since sizeof(struct ethhdr) is 14.
+                         */
+                        frame = dev_alloc_skb(hlen + subframe_len + 2);
+                        if (!frame)
+                                goto purge;
+
+                        skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
+                        memcpy(skb_put(frame, ntohs(len)), skb->data,
+                                ntohs(len));
+
+                        eth = (struct ethhdr *)skb_pull(skb, ntohs(len) +
+                                                        padding);
+                        if (!eth) {
+                                dev_kfree_skb(frame);
+                                goto purge;
+                        }
+                }
+
+                skb_reset_network_header(frame);
+                frame->dev = skb->dev;
+                frame->priority = skb->priority;
+
+                payload = frame->data;
+                ethertype = (payload[6] << 8) | payload[7];
+
+                if (likely((compare_ether_addr(payload, rfc1042_header) == 0 &&
+                            ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
+                           compare_ether_addr(payload,
+                                              bridge_tunnel_header) == 0)) {
+                        /* remove RFC1042 or Bridge-Tunnel
+                         * encapsulation and replace EtherType */
+                        skb_pull(frame, 6);
+                        memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
+                        memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
+                } else {
+                        memcpy(skb_push(frame, sizeof(__be16)), &len,
+                                sizeof(__be16));
+                        memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
+                        memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
+                }
+                __skb_queue_tail(list, frame);
+        }
+
+        return;
+
+ purge:
+        __skb_queue_purge(list);
+ out:
+        dev_kfree_skb(skb);
+}
+EXPORT_SYMBOL(ieee80211_amsdu_to_8023s);
+
 /* Given a data frame determine the 802.1p/1d tag to use. */
 unsigned int cfg80211_classify8021d(struct sk_buff *skb)
 {