xnu-1228.9.59.tar.gz

[apple/xnu.git] / bsd / net / ether_if_module.c

diff --git a/bsd/net/ether_if_module.c b/bsd/net/ether_if_module.c
index 8a4da6c6c951a5f05941e90ce7e9d6fe6a8ba19d..c2607645ab1a2321b31dbdd1d228f5c8f265202c 100644 (file)
--- a/bsd/net/ether_if_module.c
+++ b/bsd/net/ether_if_module.c
@@ -406,11 +406,35 @@ ether_demux(
                }
        }
        
-       /* Quick check for VLAN */
-       if ((m->m_pkthdr.csum_flags & CSUM_VLAN_TAG_VALID) != 0 ||
-               ether_type == htons(ETHERTYPE_VLAN)) {
-               *protocol_family = PF_VLAN;
-               return 0;
+       /* check for VLAN */
+       if ((m->m_pkthdr.csum_flags & CSUM_VLAN_TAG_VALID) != 0) {
+               if (EVL_VLANOFTAG(m->m_pkthdr.vlan_tag) != 0) {
+                       *protocol_family = PF_VLAN;
+                       return (0);
+               }
+               /* the packet is just priority-tagged, clear the bit */
+               m->m_pkthdr.csum_flags &= ~CSUM_VLAN_TAG_VALID;
+       }
+       else if (ether_type == htons(ETHERTYPE_VLAN)) {
+               struct ether_vlan_header *      evl;
+
+               evl = (struct ether_vlan_header *)frame_header;
+               if (m->m_len < ETHER_VLAN_ENCAP_LEN
+                   || ntohs(evl->evl_proto) == ETHERTYPE_VLAN
+                   || EVL_VLANOFTAG(ntohs(evl->evl_tag)) != 0) {
+                       *protocol_family = PF_VLAN;
+                       return 0;
+               }
+               /* the packet is just priority-tagged */
+
+               /* make the encapsulated ethertype the actual ethertype */
+               ether_type = evl->evl_encap_proto = evl->evl_proto;
+
+               /* remove the encapsulation header */
+               m->m_len -= ETHER_VLAN_ENCAP_LEN;
+               m->m_data += ETHER_VLAN_ENCAP_LEN;
+               m->m_pkthdr.len -= ETHER_VLAN_ENCAP_LEN;
+               m->m_pkthdr.csum_flags = 0; /* can't trust hardware checksum */
        }
        
        data = mtod(m, u_int8_t*);