1 files changed, 97 insertions, 2 deletions

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)
 {