2 files changed, 54 insertions, 1 deletions

diff --git a/net/core/dev.c b/net/core/dev.c
index 1c15b189c52b..b65a5051361f 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -3602,7 +3602,7 @@ another_round:
 
         if (skb->protocol == cpu_to_be16(ETH_P_8021Q) ||
             skb->protocol == cpu_to_be16(ETH_P_8021AD)) {
-                skb = vlan_untag(skb);
+                skb = skb_vlan_untag(skb);
                 if (unlikely(!skb))
                         goto unlock;
         }
diff --git a/net/core/skbuff.c b/net/core/skbuff.c
index 224506a6fa80..163b673f9e62 100644
--- a/net/core/skbuff.c
+++ b/net/core/skbuff.c
@@ -62,6 +62,7 @@
 #include <linux/scatterlist.h>
 #include <linux/errqueue.h>
 #include <linux/prefetch.h>
+#include <linux/if_vlan.h>
 
 #include <net/protocol.h>
 #include <net/dst.h>
@@ -3973,3 +3974,55 @@ unsigned int skb_gso_transport_seglen(const struct sk_buff *skb)
         return shinfo->gso_size;
 }
 EXPORT_SYMBOL_GPL(skb_gso_transport_seglen);
+
+static struct sk_buff *skb_reorder_vlan_header(struct sk_buff *skb)
+{
+        if (skb_cow(skb, skb_headroom(skb)) < 0) {
+                kfree_skb(skb);
+                return NULL;
+        }
+
+        memmove(skb->data - ETH_HLEN, skb->data - VLAN_ETH_HLEN, 2 * ETH_ALEN);
+        skb->mac_header += VLAN_HLEN;
+        return skb;
+}
+
+struct sk_buff *skb_vlan_untag(struct sk_buff *skb)
+{
+        struct vlan_hdr *vhdr;
+        u16 vlan_tci;
+
+        if (unlikely(vlan_tx_tag_present(skb))) {
+                /* vlan_tci is already set-up so leave this for another time */
+                return skb;
+        }
+
+        skb = skb_share_check(skb, GFP_ATOMIC);
+        if (unlikely(!skb))
+                goto err_free;
+
+        if (unlikely(!pskb_may_pull(skb, VLAN_HLEN)))
+                goto err_free;
+
+        vhdr = (struct vlan_hdr *)skb->data;
+        vlan_tci = ntohs(vhdr->h_vlan_TCI);
+        __vlan_hwaccel_put_tag(skb, skb->protocol, vlan_tci);
+
+        skb_pull_rcsum(skb, VLAN_HLEN);
+        vlan_set_encap_proto(skb, vhdr);
+
+        skb = skb_reorder_vlan_header(skb);
+        if (unlikely(!skb))
+                goto err_free;
+
+        skb_reset_network_header(skb);
+        skb_reset_transport_header(skb);
+        skb_reset_mac_len(skb);
+
+        return skb;
+
+err_free:
+        kfree_skb(skb);
+        return NULL;
+}
+EXPORT_SYMBOL(skb_vlan_untag);