]> The Tcpdump Group git mirrors - libpcap/blobdiff - gencode.c
Tony Li's changes, from FreeBSD, to support filtering for OSI packets
[libpcap] / gencode.c
index 670c9f0eb359c9895bd37be62b52fd97232e9b27..ea844903b1a8b74c85109358f532d1565ef29abb 100644 (file)
--- a/gencode.c
+++ b/gencode.c
  */
 #ifndef lint
 static const char rcsid[] =
-    "@(#) $Header: /tcpdump/master/libpcap/gencode.c,v 1.105 2000-04-01 11:44:59 assar Exp $ (LBL)";
+    "@(#) $Header: /tcpdump/master/libpcap/gencode.c,v 1.129 2000-10-28 09:30:21 guy Exp $ (LBL)";
+#endif
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
 #endif
 
 #include <sys/types.h>
@@ -31,28 +35,22 @@ static const char rcsid[] =
 #include <sys/param.h>
 #endif
 
-#if __STDC__
 struct mbuf;
 struct rtentry;
-#endif
-
 #include <net/if.h>
 
 #include <netinet/in.h>
-#include <netinet/if_ether.h>
 
 #include <stdlib.h>
+#include <string.h>
 #include <memory.h>
 #include <setjmp.h>
-#if __STDC__
 #include <stdarg.h>
-#else
-#include <varargs.h>
-#endif
 
 #include "pcap-int.h"
 
 #include "ethertype.h"
+#include "nlpid.h"
 #include "gencode.h"
 #include "ppp.h"
 #include <pcap-namedb.h>
@@ -61,7 +59,10 @@ struct rtentry;
 #include <sys/socket.h>
 #endif /*INET6*/
 
-#include "gnuc.h"
+#define LLC_ISO_LSAP   0xfe
+
+#define ETHERMTU       1500
+
 #ifdef HAVE_OS_PROTO_H
 #include "os-proto.h"
 #endif
@@ -80,24 +81,16 @@ int pcap_fddipad;
 #endif
 
 /* VARARGS */
-__dead void
-#if __STDC__
+void
 bpf_error(const char *fmt, ...)
-#else
-bpf_error(fmt, va_alist)
-       const char *fmt;
-       va_dcl
-#endif
+
 {
        va_list ap;
 
-#if __STDC__
        va_start(ap, fmt);
-#else
-       va_start(ap);
-#endif
        if (bpf_pcap != NULL)
-               (void)vsprintf(pcap_geterr(bpf_pcap), fmt, ap);
+               (void)vsnprintf(pcap_geterr(bpf_pcap), PCAP_ERRBUF_SIZE,
+                   fmt, ap);
        va_end(ap);
        longjmp(top_ctx, 1);
        /* NOTREACHED */
@@ -136,24 +129,29 @@ static inline void syntax(void);
 static void backpatch(struct block *, struct block *);
 static void merge(struct block *, struct block *);
 static struct block *gen_cmp(u_int, u_int, bpf_int32);
+static struct block *gen_cmp_gt(u_int, u_int, bpf_int32);
 static struct block *gen_mcmp(u_int, u_int, bpf_int32, bpf_u_int32);
 static struct block *gen_bcmp(u_int, u_int, const u_char *);
 static struct block *gen_uncond(int);
 static inline struct block *gen_true(void);
 static inline struct block *gen_false(void);
 static struct block *gen_linktype(int);
+static struct block *gen_snap(bpf_u_int32, bpf_u_int32, u_int);
 static struct block *gen_hostop(bpf_u_int32, bpf_u_int32, int, int, u_int, u_int);
 #ifdef INET6
 static struct block *gen_hostop6(struct in6_addr *, struct in6_addr *, int, int, u_int, u_int);
 #endif
 static struct block *gen_ehostop(const u_char *, int);
 static struct block *gen_fhostop(const u_char *, int);
+static struct block *gen_thostop(const u_char *, int);
 static struct block *gen_dnhostop(bpf_u_int32, int, u_int);
 static struct block *gen_host(bpf_u_int32, bpf_u_int32, int, int);
 #ifdef INET6
 static struct block *gen_host6(struct in6_addr *, struct in6_addr *, int, int);
 #endif
+#ifndef INET6
 static struct block *gen_gateway(const u_char *, bpf_u_int32 **, int, int);
+#endif
 static struct block *gen_ipfrag(void);
 static struct block *gen_portatom(int, bpf_int32);
 #ifdef INET6
@@ -166,6 +164,7 @@ struct block *gen_portop6(int, int, int);
 static struct block *gen_port6(int, int, int);
 #endif
 static int lookup_proto(const char *, int);
+static struct block *gen_protochain(int, int, int);
 static struct block *gen_proto(int, int, int);
 static struct slist *xfer_to_x(struct arth *);
 static struct slist *xfer_to_a(struct arth *);
@@ -225,7 +224,7 @@ sdup(s)
        int n = strlen(s) + 1;
        char *cp = newchunk(n);
 
-       strcpy(cp, s);
+       strlcpy(cp, s, n);
        return (cp);
 }
 
@@ -286,12 +285,27 @@ pcap_compile(pcap_t *p, struct bpf_program *program,
        root = NULL;
        bpf_pcap = p;
        if (setjmp(top_ctx)) {
+               lex_cleanup();
                freechunks();
                return (-1);
        }
 
        netmask = mask;
+       
+       /* On Linux we do not use the BPF filter to truncate the packet
+        * since the kernel provides other ways for that. In fact if we
+        * are using the packet filter for that duty we will be unable 
+        * to acquire the original packet size.  -- Torsten */
+#ifndef linux
        snaplen = pcap_snapshot(p);
+#else
+       snaplen = 0xffff;
+#endif
+       if (snaplen == 0) {
+               snprintf(p->errbuf, PCAP_ERRBUF_SIZE,
+                        "snaplen of 0 rejects all packets");
+               return -1;
+       }
 
        lex_init(buf ? buf : "");
        init_linktype(pcap_datalink(p));
@@ -312,6 +326,7 @@ pcap_compile(pcap_t *p, struct bpf_program *program,
        program->bf_insns = icode_to_fcode(root, &len);
        program->bf_len = len;
 
+       lex_cleanup();
        freechunks();
        return (0);
 }
@@ -364,6 +379,20 @@ pcap_compile_nopcap(int snaplen_arg, int linktype_arg,
        return (0);
 }
 
+/*
+ * Clean up a "struct bpf_program" by freeing all the memory allocated
+ * in it.
+ */
+void
+pcap_freecode(struct bpf_program *program)
+{
+       program->bf_len = 0;
+       if (program->bf_insns != NULL) {
+               free((char *)program->bf_insns);
+               program->bf_insns = NULL;
+       }
+}
+
 /*
  * Backpatch the blocks in 'list' to 'target'.  The 'sense' field indicates
  * which of the jt and jf fields has been resolved and which is a pointer
@@ -464,6 +493,24 @@ gen_cmp(offset, size, v)
        return b;
 }
 
+static struct block *
+gen_cmp_gt(offset, size, v)
+       u_int offset, size;
+       bpf_int32 v;
+{
+       struct slist *s;
+       struct block *b;
+
+       s = new_stmt(BPF_LD|BPF_ABS|size);
+       s->s.k = offset;
+
+       b = new_block(JMP(BPF_JGT));
+       b->stmts = s;
+       b->s.k = v;
+
+       return b;
+}
+
 static struct block *
 gen_mcmp(offset, size, v, mask)
        u_int offset, size;
@@ -563,7 +610,8 @@ init_linktype(type)
                return;
 
        case DLT_PPP:
-       case DLT_CHDLC:
+       case DLT_C_HDLC:
+       case DLT_PPP_SERIAL:
                off_linktype = 2;
                off_nl = 4;
                return;
@@ -578,6 +626,8 @@ init_linktype(type)
                 * FDDI doesn't really have a link-level type field.
                 * We assume that SSAP = SNAP is being used and pick
                 * out the encapsulated Ethernet type.
+                *
+                * XXX - should we generate code to check for SNAP?
                 */
                off_linktype = 19;
 #ifdef PCAP_FDDIPAD
@@ -590,6 +640,28 @@ init_linktype(type)
                return;
 
        case DLT_IEEE802:
+               /*
+                * Token Ring doesn't really have a link-level type field.
+                * We assume that SSAP = SNAP is being used and pick
+                * out the encapsulated Ethernet type.
+                *
+                * XXX - should we generate code to check for SNAP?
+                *
+                * XXX - the header is actually variable-length.
+                * Some various Linux patched versions gave 38
+                * as "off_linktype" and 40 as "off_nl"; however,
+                * if a token ring packet has *no* routing
+                * information, i.e. is not source-routed, the correct
+                * values are 20 and 22, as they are in the vanilla code.
+                *
+                * A packet is source-routed iff the uppermost bit
+                * of the first byte of the source address, at an
+                * offset of 8, has the uppermost bit set.  If the
+                * packet is source-routed, the total number of bytes
+                * of routing information is 2 plus bits 0x1F00 of
+                * the 16-bit value at an offset of 14 (shifted right
+                * 8 - figure out which byte that is).
+                */
                off_linktype = 20;
                off_nl = 22;
                return;
@@ -607,6 +679,11 @@ init_linktype(type)
                off_linktype = -1;
                off_nl = 0;
                return;
+
+       case DLT_ATM_CLIP:      /* Linux ATM defines this */
+               off_linktype = 6;
+               off_nl = 8;
+               return;
        }
        bpf_error("unknown data link type 0x%x", linktype);
        /* NOTREACHED */
@@ -645,25 +722,114 @@ gen_linktype(proto)
 {
        struct block *b0, *b1;
 
-       /* If we're not using encapsulation and checking for IP, we're done */
-       if (off_linktype == -1 && proto == ETHERTYPE_IP)
+       /* If we're not using encapsulation, we're done */
+       if (off_linktype == -1)
                return gen_true();
 
        switch (linktype) {
 
+       case DLT_EN10MB:
+               /*
+                * XXX - handle other LLC-encapsulated protocols here
+                * (IPX, OSI)?
+                */
+               switch (proto) {
+
+               case LLC_ISO_LSAP:
+                       /*
+                        * OSI protocols always use 802.2 encapsulation.
+                        */
+                       b0 = gen_cmp_gt(off_linktype, BPF_H, ETHERMTU);
+                       gen_not(b0);
+                       b1 = gen_cmp(off_linktype + 2, BPF_H, (long)
+                                    ((LLC_ISO_LSAP << 8) | LLC_ISO_LSAP));
+                       gen_and(b0, b1);
+                       return b1;
+
+               case ETHERTYPE_ATALK:
+               case ETHERTYPE_AARP:
+                       /*
+                        * EtherTalk (AppleTalk protocols on Ethernet link
+                        * layer) may use 802.2 encapsulation.
+                        */
+
+                       /*
+                        * Check for 802.2 encapsulation (EtherTalk phase 2?);
+                        * we check for an Ethernet type field less than
+                        * 1500, which means it's an 802.3 length field.
+                        */
+                       b0 = gen_cmp_gt(off_linktype, BPF_H, ETHERMTU);
+                       gen_not(b0);
+
+                       /*
+                        * 802.2-encapsulated ETHERTYPE_ATALK packets are
+                        * SNAP packets with an organization code of
+                        * 0x080007 (Apple, for Appletalk) and a protocol
+                        * type of ETHERTYPE_ATALK (Appletalk).
+                        *
+                        * 802.2-encapsulated ETHERTYPE_AARP packets are
+                        * SNAP packets with an organization code of
+                        * 0x000000 (encapsulated Ethernet) and a protocol
+                        * type of ETHERTYPE_AARP (Appletalk ARP).
+                        */
+                       if (proto == ETHERTYPE_ATALK)
+                               b1 = gen_snap(0x080007, ETHERTYPE_ATALK, 14);
+                       else    /* proto == ETHERTYPE_AARP */
+                               b1 = gen_snap(0x000000, ETHERTYPE_AARP, 14);
+                       gen_and(b0, b1);
+
+                       /*
+                        * Check for Ethernet encapsulation (Ethertalk
+                        * phase 1?); we just check for the Ethernet
+                        * protocol type.
+                        */
+                       b0 = gen_cmp(off_linktype, BPF_H, (bpf_int32)proto);
+
+                       gen_or(b0, b1);
+                       return b1;
+               }
+               break;
+
        case DLT_SLIP:
                return gen_false();
 
        case DLT_PPP:
-               if (proto == ETHERTYPE_IP)
+       case DLT_PPP_SERIAL:
+               /*
+                * We use Ethernet protocol types inside libpcap;
+                * map them to the corresponding PPP protocol types.
+                */
+               switch (proto) {
+
+               case ETHERTYPE_IP:
                        proto = PPP_IP;                 /* XXX was 0x21 */
+                       break;
+
 #ifdef INET6
-               else if (proto == ETHERTYPE_IPV6)
+               case ETHERTYPE_IPV6:
                        proto = PPP_IPV6;
+                       break;
 #endif
+
+               case ETHERTYPE_DN:
+                       proto = PPP_DECNET;
+                       break;
+
+               case ETHERTYPE_ATALK:
+                       proto = PPP_APPLE;
+                       break;
+
+               case ETHERTYPE_NS:
+                       proto = PPP_NS;
+                       break;
+               }
                break;
 
        case DLT_PPP_BSDOS:
+               /*
+                * We use Ethernet protocol types inside libpcap;
+                * map them to the corresponding PPP protocol types.
+                */
                switch (proto) {
 
                case ETHERTYPE_IP:
@@ -709,6 +875,32 @@ gen_linktype(proto)
        return gen_cmp(off_linktype, BPF_H, (bpf_int32)proto);
 }
 
+/*
+ * Check for an LLC SNAP packet with a given organization code and
+ * protocol type; we check the entire contents of the 802.2 LLC and
+ * snap headers, checking for a DSAP of 0xAA, an SSAP of 0xAA, and
+ * a control field of 0x03 in the LLC header, and for the specified
+ * organization code and protocol type in the SNAP header.
+ */
+static struct block *
+gen_snap(orgcode, ptype, offset)
+       bpf_u_int32 orgcode;
+       bpf_u_int32 ptype;
+       u_int offset;
+{
+       u_char snapblock[8];
+
+       snapblock[0] = 0xAA;    /* DSAP = SNAP */
+       snapblock[1] = 0xAA;    /* SSAP = SNAP */
+       snapblock[2] = 0x03;    /* control = UI */
+       snapblock[3] = (orgcode >> 16); /* upper 8 bits of organization code */
+       snapblock[4] = (orgcode >> 8);  /* middle 8 bits of organization code */
+       snapblock[5] = (orgcode >> 0);  /* lower 8 bits of organization code */
+       snapblock[6] = (ptype >> 8);    /* upper 8 bits of protocol type */
+       snapblock[7] = (ptype >> 0);    /* lower 8 bits of protocol type */
+       return gen_bcmp(offset, 8, snapblock);
+}
+
 static struct block *
 gen_hostop(addr, mask, dir, proto, src_off, dst_off)
        bpf_u_int32 addr;
@@ -878,6 +1070,40 @@ gen_fhostop(eaddr, dir)
        /* NOTREACHED */
 }
 
+/*
+ * Like gen_ehostop, but for DLT_IEEE802 (Token Ring)
+ */
+static struct block *
+gen_thostop(eaddr, dir)
+       register const u_char *eaddr;
+       register int dir;
+{
+       register struct block *b0, *b1;
+
+       switch (dir) {
+       case Q_SRC:
+               return gen_bcmp(8, 6, eaddr);
+
+       case Q_DST:
+               return gen_bcmp(2, 6, eaddr);
+
+       case Q_AND:
+               b0 = gen_thostop(eaddr, Q_SRC);
+               b1 = gen_thostop(eaddr, Q_DST);
+               gen_and(b0, b1);
+               return b1;
+
+       case Q_DEFAULT:
+       case Q_OR:
+               b0 = gen_thostop(eaddr, Q_SRC);
+               b1 = gen_thostop(eaddr, Q_DST);
+               gen_or(b0, b1);
+               return b1;
+       }
+       abort();
+       /* NOTREACHED */
+}
+
 /*
  * This is quite tricky because there may be pad bytes in front of the
  * DECNET header, and then there are two possible data packet formats that
@@ -933,6 +1159,9 @@ gen_dnhostop(addr, dir, base_off)
                gen_or(b0, b1);
                return b1;
 
+       case Q_ISO:
+               bpf_error("ISO host filtering not implemented");
+               
        default:
                abort();
        }
@@ -979,10 +1208,12 @@ gen_host(addr, mask, proto, dir)
 
        case Q_DEFAULT:
                b0 = gen_host(addr, mask, Q_IP, dir);
-               b1 = gen_host(addr, mask, Q_ARP, dir);
-               gen_or(b0, b1);
-               b0 = gen_host(addr, mask, Q_RARP, dir);
-               gen_or(b1, b0);
+               if (off_linktype != -1) {
+                   b1 = gen_host(addr, mask, Q_ARP, dir);
+                   gen_or(b0, b1);
+                   b0 = gen_host(addr, mask, Q_RARP, dir);
+                   gen_or(b1, b0);
+               }
                return b0;
 
        case Q_IP:
@@ -1018,6 +1249,9 @@ gen_host(addr, mask, proto, dir)
        case Q_ATALK:
                bpf_error("ATALK host filtering not implemented");
 
+       case Q_AARP:
+               bpf_error("AARP host filtering not implemented");
+
        case Q_DECNET:
                return gen_dnhostop(addr, dir, off_nl);
 
@@ -1061,8 +1295,6 @@ gen_host6(addr, mask, proto, dir)
        int proto;
        int dir;
 {
-       struct block *b0, *b1;
-
        switch (proto) {
 
        case Q_DEFAULT:
@@ -1098,6 +1330,9 @@ gen_host6(addr, mask, proto, dir)
        case Q_ATALK:
                bpf_error("ATALK host filtering not implemented");
 
+       case Q_AARP:
+               bpf_error("AARP host filtering not implemented");
+
        case Q_DECNET:
                bpf_error("'decnet' modifier applied to ip6 host");
 
@@ -1133,6 +1368,7 @@ gen_host6(addr, mask, proto, dir)
 }
 #endif /*INET6*/
 
+#ifndef INET6
 static struct block *
 gen_gateway(eaddr, alist, proto, dir)
        const u_char *eaddr;
@@ -1154,9 +1390,11 @@ gen_gateway(eaddr, alist, proto, dir)
                        b0 = gen_ehostop(eaddr, Q_OR);
                else if (linktype == DLT_FDDI)
                        b0 = gen_fhostop(eaddr, Q_OR);
+               else if (linktype == DLT_IEEE802)
+                       b0 = gen_thostop(eaddr, Q_OR);
                else
                        bpf_error(
-                           "'gateway' supported only on ethernet or FDDI");
+                           "'gateway' supported only on ethernet, FDDI or token ring");
 
                b1 = gen_host(**alist++, 0xffffffff, proto, Q_OR);
                while (*alist) {
@@ -1171,12 +1409,16 @@ gen_gateway(eaddr, alist, proto, dir)
        bpf_error("illegal modifier of 'gateway'");
        /* NOTREACHED */
 }
+#endif
 
 struct block *
 gen_proto_abbrev(proto)
        int proto;
 {
-       struct block *b0, *b1;
+#ifdef INET6
+       struct block *b0;
+#endif
+       struct block *b1;
 
        switch (proto) {
 
@@ -1246,6 +1488,10 @@ gen_proto_abbrev(proto)
                b1 =  gen_linktype(ETHERTYPE_ATALK);
                break;
 
+       case Q_AARP:
+               b1 =  gen_linktype(ETHERTYPE_AARP);
+               break;
+
        case Q_DECNET:
                b1 =  gen_linktype(ETHERTYPE_DN);
                break;
@@ -1301,6 +1547,18 @@ gen_proto_abbrev(proto)
 #endif
                break;
 
+       case Q_ISO:
+               b1 = gen_linktype(LLC_ISO_LSAP);
+               break;
+
+       case Q_ESIS:
+               b1 = gen_proto(ISO9542_ESIS, Q_ISO, Q_DEFAULT);
+               break;
+
+       case Q_ISIS:
+               b1 = gen_proto(ISO10589_ISIS, Q_ISO, Q_DEFAULT);
+               break;
+
        default:
                abort();
        }
@@ -1527,14 +1785,17 @@ lookup_proto(name, proto)
        return v;
 }
 
+#if 0
 struct stmt *
 gen_joinsp(s, n)
        struct stmt **s;
        int n;
 {
+       return NULL;
 }
+#endif
 
-struct block *
+static struct block *
 gen_protochain(v, proto, dir)
        int v;
        int proto;
@@ -1544,7 +1805,7 @@ gen_protochain(v, proto, dir)
        return gen_proto(v, proto, dir);
 #else
        struct block *b0, *b;
-       struct slist *s[100], *sp;
+       struct slist *s[100];
        int fix2, fix3, fix4, fix5;
        int ahcheck, again, end;
        int i, max;
@@ -1869,6 +2130,12 @@ gen_proto(v, proto, dir)
                gen_and(b0, b1);
                return b1;
 
+       case Q_ISO:
+               b0 = gen_linktype(LLC_ISO_LSAP);
+               b1 = gen_cmp(off_nl + 3, BPF_B, (long)v);
+               gen_and(b0, b1);
+               return b1;
+
        case Q_ARP:
                bpf_error("arp does not encapsulate another protocol");
                /* NOTREACHED */
@@ -1965,8 +2232,10 @@ gen_scode(name, q)
        int dir = q.dir;
        int tproto;
        u_char *eaddr;
-       bpf_u_int32 mask, addr, **alist;
-#ifdef INET6
+       bpf_u_int32 mask, addr;
+#ifndef INET6
+       bpf_u_int32 **alist;
+#else
        int tproto6;
        struct sockaddr_in *sin;
        struct sockaddr_in6 *sin6;
@@ -2009,9 +2278,16 @@ gen_scode(name, q)
                                            "unknown FDDI host '%s'", name);
                                return gen_fhostop(eaddr, dir);
 
+                       case DLT_IEEE802:
+                               eaddr = pcap_ether_hostton(name);
+                               if (eaddr == NULL)
+                                       bpf_error(
+                                           "unknown token ring host '%s'", name);
+                               return gen_thostop(eaddr, dir);
+
                        default:
                                bpf_error(
-                       "only ethernet/FDDI supports link-level host name");
+                       "only ethernet/FDDI/token ring supports link-level host name");
                                break;
                        }
                } else if (proto == Q_DECNET) {
@@ -2039,7 +2315,7 @@ gen_scode(name, q)
                        return b;
 #else
                        memset(&mask128, 0xff, sizeof(mask128));
-                       res0 = res = pcap_nametoaddr(name);
+                       res0 = res = pcap_nametoaddrinfo(name);
                        if (res == NULL)
                                bpf_error("unknown host '%s'", name);
                        b = tmp = NULL;
@@ -2293,7 +2569,7 @@ gen_mcode6(s1, s2, masklen, q)
        if (s2)
                bpf_error("no mask %s supported", s2);
 
-       res = pcap_nametoaddr(s1);
+       res = pcap_nametoaddrinfo(s1);
        if (!res)
                bpf_error("invalid ip6 address %s", s1);
        if (res->ai_next)
@@ -2345,6 +2621,8 @@ gen_ecode(eaddr, q)
                        return gen_ehostop(eaddr, (int)q.dir);
                if (linktype == DLT_FDDI)
                        return gen_fhostop(eaddr, (int)q.dir);
+               if (linktype == DLT_IEEE802)
+                       return gen_thostop(eaddr, (int)q.dir);
        }
        bpf_error("ethernet address used in non-ether expression");
        /* NOTREACHED */
@@ -2673,6 +2951,9 @@ gen_greater(n)
        return gen_len(BPF_JGE, n);
 }
 
+/*
+ * Actually, this is less than or equal.
+ */
 struct block *
 gen_less(n)
        int n;
@@ -2742,6 +3023,8 @@ gen_broadcast(proto)
                        return gen_ehostop(ebroadcast, Q_DST);
                if (linktype == DLT_FDDI)
                        return gen_fhostop(ebroadcast, Q_DST);
+               if (linktype == DLT_IEEE802)
+                       return gen_thostop(ebroadcast, Q_DST);
                bpf_error("not a broadcast link");
                break;
 
@@ -2789,6 +3072,10 @@ gen_multicast(proto)
                        b0->stmts = s;
                        return b0;
                }
+
+               /* TODO - check how token ring handles multicast */
+               /* if (linktype == DLT_IEEE802) ... */
+
                /* Link not known to support multicasts */
                break;
 
@@ -2827,3 +3114,50 @@ gen_inbound(dir)
                          dir);
        return (b0);
 }
+
+/*
+ * support IEEE 802.1Q VLAN trunk over ethernet
+ */
+struct block *
+gen_vlan(vlan_num)
+       int vlan_num;
+{
+       static u_int    orig_linktype = -1, orig_nl = -1;
+       struct  block   *b0;
+
+       /*
+        * Change the offsets to point to the type and data fields within
+        * the VLAN packet.  This is somewhat of a kludge.
+        */
+       if (orig_nl == (u_int)-1) {
+               orig_linktype = off_linktype;   /* save original values */
+               orig_nl = off_nl;
+
+               switch (linktype) {
+
+               case DLT_EN10MB:
+                       off_linktype = 16;
+                       off_nl = 18;
+                       break;
+
+               default:
+                       bpf_error("no VLAN support for data link type %d",
+                                 linktype);
+                       /*NOTREACHED*/
+               }
+       }
+
+       /* check for VLAN */
+       b0 = gen_cmp(orig_linktype, BPF_H, (bpf_int32)ETHERTYPE_8021Q);
+
+       /* If a specific VLAN is requested, check VLAN id */
+       if (vlan_num >= 0) {
+               struct block *b1;
+
+               b1 = gen_cmp(orig_nl, BPF_H, (bpf_int32)vlan_num);
+               gen_and(b0, b1);
+               b0 = b1;
+       }
+
+       return (b0);
+}