1 files changed, 53 insertions, 21 deletions

diff --git a/include/linux/if_vlan.h b/include/linux/if_vlan.h
index 515a35e2a48a..b11b28a30b9e 100644
--- a/include/linux/if_vlan.h
+++ b/include/linux/if_vlan.h
@@ -78,9 +78,9 @@ static inline bool is_vlan_dev(struct net_device *dev)
         return dev->priv_flags & IFF_802_1Q_VLAN;
 }
 
-#define vlan_tx_tag_present(__skb)      ((__skb)->vlan_tci & VLAN_TAG_PRESENT)
-#define vlan_tx_tag_get(__skb)          ((__skb)->vlan_tci & ~VLAN_TAG_PRESENT)
-#define vlan_tx_tag_get_id(__skb)       ((__skb)->vlan_tci & VLAN_VID_MASK)
+#define skb_vlan_tag_present(__skb)     ((__skb)->vlan_tci & VLAN_TAG_PRESENT)
+#define skb_vlan_tag_get(__skb)         ((__skb)->vlan_tci & ~VLAN_TAG_PRESENT)
+#define skb_vlan_tag_get_id(__skb)      ((__skb)->vlan_tci & VLAN_VID_MASK)
 
 /**
  *      struct vlan_pcpu_stats - VLAN percpu rx/tx stats
@@ -376,7 +376,7 @@ static inline struct sk_buff *vlan_insert_tag_set_proto(struct sk_buff *skb,
 static inline struct sk_buff *__vlan_hwaccel_push_inside(struct sk_buff *skb)
 {
         skb = vlan_insert_tag_set_proto(skb, skb->vlan_proto,
-                                        vlan_tx_tag_get(skb));
+                                        skb_vlan_tag_get(skb));
         if (likely(skb))
                 skb->vlan_tci = 0;
         return skb;
@@ -393,7 +393,7 @@ static inline struct sk_buff *__vlan_hwaccel_push_inside(struct sk_buff *skb)
  */
 static inline struct sk_buff *vlan_hwaccel_push_inside(struct sk_buff *skb)
 {
-        if (vlan_tx_tag_present(skb))
+        if (skb_vlan_tag_present(skb))
                 skb = __vlan_hwaccel_push_inside(skb);
         return skb;
 }
@@ -442,8 +442,8 @@ static inline int __vlan_get_tag(const struct sk_buff *skb, u16 *vlan_tci)
 static inline int __vlan_hwaccel_get_tag(const struct sk_buff *skb,
                                          u16 *vlan_tci)
 {
-        if (vlan_tx_tag_present(skb)) {
-                *vlan_tci = vlan_tx_tag_get(skb);
+        if (skb_vlan_tag_present(skb)) {
+                *vlan_tci = skb_vlan_tag_get(skb);
                 return 0;
         } else {
                 *vlan_tci = 0;
@@ -472,27 +472,59 @@ static inline int vlan_get_tag(const struct sk_buff *skb, u16 *vlan_tci)
 /**
  * vlan_get_protocol - get protocol EtherType.
  * @skb: skbuff to query
+ * @type: first vlan protocol
+ * @depth: buffer to store length of eth and vlan tags in bytes
  *
  * Returns the EtherType of the packet, regardless of whether it is
  * vlan encapsulated (normal or hardware accelerated) or not.
  */
-static inline __be16 vlan_get_protocol(const struct sk_buff *skb)
+static inline __be16 __vlan_get_protocol(struct sk_buff *skb, __be16 type,
+                                         int *depth)
 {
-        __be16 protocol = 0;
-
-        if (vlan_tx_tag_present(skb) ||
-             skb->protocol != cpu_to_be16(ETH_P_8021Q))
-                protocol = skb->protocol;
-        else {
-                __be16 proto, *protop;
-                protop = skb_header_pointer(skb, offsetof(struct vlan_ethhdr,
-                                                h_vlan_encapsulated_proto),
-                                                sizeof(proto), &proto);
-                if (likely(protop))
-                        protocol = *protop;
+        unsigned int vlan_depth = skb->mac_len;
+
+        /* if type is 802.1Q/AD then the header should already be
+         * present at mac_len - VLAN_HLEN (if mac_len > 0), or at
+         * ETH_HLEN otherwise
+         */
+        if (type == htons(ETH_P_8021Q) || type == htons(ETH_P_8021AD)) {
+                if (vlan_depth) {
+                        if (WARN_ON(vlan_depth < VLAN_HLEN))
+                                return 0;
+                        vlan_depth -= VLAN_HLEN;
+                } else {
+                        vlan_depth = ETH_HLEN;
+                }
+                do {
+                        struct vlan_hdr *vh;
+
+                        if (unlikely(!pskb_may_pull(skb,
+                                                    vlan_depth + VLAN_HLEN)))
+                                return 0;
+
+                        vh = (struct vlan_hdr *)(skb->data + vlan_depth);
+                        type = vh->h_vlan_encapsulated_proto;
+                        vlan_depth += VLAN_HLEN;
+                } while (type == htons(ETH_P_8021Q) ||
+                         type == htons(ETH_P_8021AD));
         }
 
-        return protocol;
+        if (depth)
+                *depth = vlan_depth;
+
+        return type;
+}
+
+/**
+ * vlan_get_protocol - get protocol EtherType.
+ * @skb: skbuff to query
+ *
+ * Returns the EtherType of the packet, regardless of whether it is
+ * vlan encapsulated (normal or hardware accelerated) or not.
+ */
+static inline __be16 vlan_get_protocol(struct sk_buff *skb)
+{
+        return __vlan_get_protocol(skb, skb->protocol, NULL);
 }
 
 static inline void vlan_set_encap_proto(struct sk_buff *skb,