xref: /freebsd/sys/net/if_ethersubr.c (revision 39beb93c3f8bdbf72a61fda42300b5ebed7390c8)
1 /*-
2  * Copyright (c) 1982, 1989, 1993
3  *	The Regents of the University of California.  All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 4. Neither the name of the University nor the names of its contributors
14  *    may be used to endorse or promote products derived from this software
15  *    without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27  * SUCH DAMAGE.
28  *
29  *	@(#)if_ethersubr.c	8.1 (Berkeley) 6/10/93
30  * $FreeBSD$
31  */
32 
33 #include "opt_atalk.h"
34 #include "opt_inet.h"
35 #include "opt_inet6.h"
36 #include "opt_ipx.h"
37 #include "opt_route.h"
38 #include "opt_mac.h"
39 #include "opt_netgraph.h"
40 #include "opt_carp.h"
41 #include "opt_mbuf_profiling.h"
42 
43 #include <sys/param.h>
44 #include <sys/systm.h>
45 #include <sys/kernel.h>
46 #include <sys/lock.h>
47 #include <sys/malloc.h>
48 #include <sys/module.h>
49 #include <sys/mbuf.h>
50 #include <sys/random.h>
51 #include <sys/rwlock.h>
52 #include <sys/socket.h>
53 #include <sys/sockio.h>
54 #include <sys/sysctl.h>
55 #include <sys/vimage.h>
56 
57 #include <net/if.h>
58 #include <net/if_arp.h>
59 #include <net/netisr.h>
60 #include <net/route.h>
61 #include <net/if_llc.h>
62 #include <net/if_dl.h>
63 #include <net/if_types.h>
64 #include <net/bpf.h>
65 #include <net/ethernet.h>
66 #include <net/if_bridgevar.h>
67 #include <net/if_vlan_var.h>
68 #include <net/if_llatbl.h>
69 #include <net/pf_mtag.h>
70 #include <net/vnet.h>
71 
72 #if defined(INET) || defined(INET6)
73 #include <netinet/in.h>
74 #include <netinet/in_var.h>
75 #include <netinet/if_ether.h>
76 #include <netinet/ip_fw.h>
77 #include <netinet/ip_dummynet.h>
78 #include <netinet/vinet.h>
79 #endif
80 #ifdef INET6
81 #include <netinet6/nd6.h>
82 #endif
83 
84 #ifdef DEV_CARP
85 #include <netinet/ip_carp.h>
86 #endif
87 
88 #ifdef IPX
89 #include <netipx/ipx.h>
90 #include <netipx/ipx_if.h>
91 #endif
92 
93 int (*ef_inputp)(struct ifnet*, struct ether_header *eh, struct mbuf *m);
94 int (*ef_outputp)(struct ifnet *ifp, struct mbuf **mp,
95 		struct sockaddr *dst, short *tp, int *hlen);
96 
97 #ifdef NETATALK
98 #include <netatalk/at.h>
99 #include <netatalk/at_var.h>
100 #include <netatalk/at_extern.h>
101 
102 #define llc_snap_org_code llc_un.type_snap.org_code
103 #define llc_snap_ether_type llc_un.type_snap.ether_type
104 
105 extern u_char	at_org_code[3];
106 extern u_char	aarp_org_code[3];
107 #endif /* NETATALK */
108 
109 #include <security/mac/mac_framework.h>
110 
111 #ifdef CTASSERT
112 CTASSERT(sizeof (struct ether_header) == ETHER_ADDR_LEN * 2 + 2);
113 CTASSERT(sizeof (struct ether_addr) == ETHER_ADDR_LEN);
114 #endif
115 
116 /* netgraph node hooks for ng_ether(4) */
117 void	(*ng_ether_input_p)(struct ifnet *ifp, struct mbuf **mp);
118 void	(*ng_ether_input_orphan_p)(struct ifnet *ifp, struct mbuf *m);
119 int	(*ng_ether_output_p)(struct ifnet *ifp, struct mbuf **mp);
120 void	(*ng_ether_attach_p)(struct ifnet *ifp);
121 void	(*ng_ether_detach_p)(struct ifnet *ifp);
122 
123 void	(*vlan_input_p)(struct ifnet *, struct mbuf *);
124 
125 /* if_bridge(4) support */
126 struct mbuf *(*bridge_input_p)(struct ifnet *, struct mbuf *);
127 int	(*bridge_output_p)(struct ifnet *, struct mbuf *,
128 		struct sockaddr *, struct rtentry *);
129 void	(*bridge_dn_p)(struct mbuf *, struct ifnet *);
130 
131 /* if_lagg(4) support */
132 struct mbuf *(*lagg_input_p)(struct ifnet *, struct mbuf *);
133 
134 static const u_char etherbroadcastaddr[ETHER_ADDR_LEN] =
135 			{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
136 
137 static	int ether_resolvemulti(struct ifnet *, struct sockaddr **,
138 		struct sockaddr *);
139 
140 /* XXX: should be in an arp support file, not here */
141 MALLOC_DEFINE(M_ARPCOM, "arpcom", "802.* interface internals");
142 
143 #define	ETHER_IS_BROADCAST(addr) \
144 	(bcmp(etherbroadcastaddr, (addr), ETHER_ADDR_LEN) == 0)
145 
146 #define senderr(e) do { error = (e); goto bad;} while (0)
147 
148 #if defined(INET) || defined(INET6)
149 int
150 ether_ipfw_chk(struct mbuf **m0, struct ifnet *dst,
151 	struct ip_fw **rule, int shared);
152 #ifdef VIMAGE_GLOBALS
153 static int ether_ipfw;
154 #endif
155 #endif
156 
157 
158 /*
159  * Ethernet output routine.
160  * Encapsulate a packet of type family for the local net.
161  * Use trailer local net encapsulation if enough data in first
162  * packet leaves a multiple of 512 bytes of data in remainder.
163  */
164 int
165 ether_output(struct ifnet *ifp, struct mbuf *m,
166 	struct sockaddr *dst, struct rtentry *rt0)
167 {
168 	short type;
169 	int error, hdrcmplt = 0;
170 	u_char esrc[ETHER_ADDR_LEN], edst[ETHER_ADDR_LEN];
171 	struct llentry *lle = NULL;
172 	struct ether_header *eh;
173 	struct pf_mtag *t;
174 	int loop_copy = 1;
175 	int hlen;	/* link layer header length */
176 
177 #ifdef MAC
178 	error = mac_ifnet_check_transmit(ifp, m);
179 	if (error)
180 		senderr(error);
181 #endif
182 
183 	M_PROFILE(m);
184 	if (ifp->if_flags & IFF_MONITOR)
185 		senderr(ENETDOWN);
186 	if (!((ifp->if_flags & IFF_UP) &&
187 	    (ifp->if_drv_flags & IFF_DRV_RUNNING)))
188 		senderr(ENETDOWN);
189 
190 	hlen = ETHER_HDR_LEN;
191 	switch (dst->sa_family) {
192 #ifdef INET
193 	case AF_INET:
194 		error = arpresolve(ifp, rt0, m, dst, edst, &lle);
195 		if (error)
196 			return (error == EWOULDBLOCK ? 0 : error);
197 		type = htons(ETHERTYPE_IP);
198 		break;
199 	case AF_ARP:
200 	{
201 		struct arphdr *ah;
202 		ah = mtod(m, struct arphdr *);
203 		ah->ar_hrd = htons(ARPHRD_ETHER);
204 
205 		loop_copy = 0; /* if this is for us, don't do it */
206 
207 		switch(ntohs(ah->ar_op)) {
208 		case ARPOP_REVREQUEST:
209 		case ARPOP_REVREPLY:
210 			type = htons(ETHERTYPE_REVARP);
211 			break;
212 		case ARPOP_REQUEST:
213 		case ARPOP_REPLY:
214 		default:
215 			type = htons(ETHERTYPE_ARP);
216 			break;
217 		}
218 
219 		if (m->m_flags & M_BCAST)
220 			bcopy(ifp->if_broadcastaddr, edst, ETHER_ADDR_LEN);
221 		else
222 			bcopy(ar_tha(ah), edst, ETHER_ADDR_LEN);
223 
224 	}
225 	break;
226 #endif
227 #ifdef INET6
228 	case AF_INET6:
229 		error = nd6_storelladdr(ifp, m, dst, (u_char *)edst, &lle);
230 		if (error)
231 			return error;
232 		type = htons(ETHERTYPE_IPV6);
233 		break;
234 #endif
235 #ifdef IPX
236 	case AF_IPX:
237 		if (ef_outputp) {
238 		    error = ef_outputp(ifp, &m, dst, &type, &hlen);
239 		    if (error)
240 			goto bad;
241 		} else
242 		    type = htons(ETHERTYPE_IPX);
243 		bcopy((caddr_t)&(((struct sockaddr_ipx *)dst)->sipx_addr.x_host),
244 		    (caddr_t)edst, sizeof (edst));
245 		break;
246 #endif
247 #ifdef NETATALK
248 	case AF_APPLETALK:
249 	  {
250 	    struct at_ifaddr *aa;
251 
252 	    if ((aa = at_ifawithnet((struct sockaddr_at *)dst)) == NULL)
253 		    senderr(EHOSTUNREACH); /* XXX */
254 	    if (!aarpresolve(ifp, m, (struct sockaddr_at *)dst, edst))
255 		    return (0);
256 	    /*
257 	     * In the phase 2 case, need to prepend an mbuf for the llc header.
258 	     */
259 	    if ( aa->aa_flags & AFA_PHASE2 ) {
260 		struct llc llc;
261 
262 		M_PREPEND(m, LLC_SNAPFRAMELEN, M_DONTWAIT);
263 		if (m == NULL)
264 			senderr(ENOBUFS);
265 		llc.llc_dsap = llc.llc_ssap = LLC_SNAP_LSAP;
266 		llc.llc_control = LLC_UI;
267 		bcopy(at_org_code, llc.llc_snap_org_code, sizeof(at_org_code));
268 		llc.llc_snap_ether_type = htons( ETHERTYPE_AT );
269 		bcopy(&llc, mtod(m, caddr_t), LLC_SNAPFRAMELEN);
270 		type = htons(m->m_pkthdr.len);
271 		hlen = LLC_SNAPFRAMELEN + ETHER_HDR_LEN;
272 	    } else {
273 		type = htons(ETHERTYPE_AT);
274 	    }
275 	    break;
276 	  }
277 #endif /* NETATALK */
278 
279 	case pseudo_AF_HDRCMPLT:
280 		hdrcmplt = 1;
281 		eh = (struct ether_header *)dst->sa_data;
282 		(void)memcpy(esrc, eh->ether_shost, sizeof (esrc));
283 		/* FALLTHROUGH */
284 
285 	case AF_UNSPEC:
286 		loop_copy = 0; /* if this is for us, don't do it */
287 		eh = (struct ether_header *)dst->sa_data;
288 		(void)memcpy(edst, eh->ether_dhost, sizeof (edst));
289 		type = eh->ether_type;
290 		break;
291 
292 	default:
293 		if_printf(ifp, "can't handle af%d\n", dst->sa_family);
294 		senderr(EAFNOSUPPORT);
295 	}
296 
297 	if (lle != NULL && (lle->la_flags & LLE_IFADDR)) {
298 		int csum_flags = 0;
299 		if (m->m_pkthdr.csum_flags & CSUM_IP)
300 			csum_flags |= (CSUM_IP_CHECKED|CSUM_IP_VALID);
301 		if (m->m_pkthdr.csum_flags & CSUM_DELAY_DATA)
302 			csum_flags |= (CSUM_DATA_VALID|CSUM_PSEUDO_HDR);
303 		if (m->m_pkthdr.csum_flags & CSUM_SCTP)
304 			csum_flags |= CSUM_SCTP_VALID;
305 		m->m_pkthdr.csum_flags |= csum_flags;
306 		m->m_pkthdr.csum_data = 0xffff;
307 		return (if_simloop(ifp, m, dst->sa_family, 0));
308 	}
309 
310 	/*
311 	 * Add local net header.  If no space in first mbuf,
312 	 * allocate another.
313 	 */
314 	M_PREPEND(m, ETHER_HDR_LEN, M_DONTWAIT);
315 	if (m == NULL)
316 		senderr(ENOBUFS);
317 	eh = mtod(m, struct ether_header *);
318 	(void)memcpy(&eh->ether_type, &type,
319 		sizeof(eh->ether_type));
320 	(void)memcpy(eh->ether_dhost, edst, sizeof (edst));
321 	if (hdrcmplt)
322 		(void)memcpy(eh->ether_shost, esrc,
323 			sizeof(eh->ether_shost));
324 	else
325 		(void)memcpy(eh->ether_shost, IF_LLADDR(ifp),
326 			sizeof(eh->ether_shost));
327 
328 	/*
329 	 * If a simplex interface, and the packet is being sent to our
330 	 * Ethernet address or a broadcast address, loopback a copy.
331 	 * XXX To make a simplex device behave exactly like a duplex
332 	 * device, we should copy in the case of sending to our own
333 	 * ethernet address (thus letting the original actually appear
334 	 * on the wire). However, we don't do that here for security
335 	 * reasons and compatibility with the original behavior.
336 	 */
337 	if ((ifp->if_flags & IFF_SIMPLEX) && loop_copy &&
338 	    ((t = pf_find_mtag(m)) == NULL || !t->routed)) {
339 		int csum_flags = 0;
340 
341 		if (m->m_pkthdr.csum_flags & CSUM_IP)
342 			csum_flags |= (CSUM_IP_CHECKED|CSUM_IP_VALID);
343 		if (m->m_pkthdr.csum_flags & CSUM_DELAY_DATA)
344 			csum_flags |= (CSUM_DATA_VALID|CSUM_PSEUDO_HDR);
345 		if (m->m_pkthdr.csum_flags & CSUM_SCTP)
346 			csum_flags |= CSUM_SCTP_VALID;
347 
348 		if (m->m_flags & M_BCAST) {
349 			struct mbuf *n;
350 
351 			/*
352 			 * Because if_simloop() modifies the packet, we need a
353 			 * writable copy through m_dup() instead of a readonly
354 			 * one as m_copy[m] would give us. The alternative would
355 			 * be to modify if_simloop() to handle the readonly mbuf,
356 			 * but performancewise it is mostly equivalent (trading
357 			 * extra data copying vs. extra locking).
358 			 *
359 			 * XXX This is a local workaround.  A number of less
360 			 * often used kernel parts suffer from the same bug.
361 			 * See PR kern/105943 for a proposed general solution.
362 			 */
363 			if ((n = m_dup(m, M_DONTWAIT)) != NULL) {
364 				n->m_pkthdr.csum_flags |= csum_flags;
365 				if (csum_flags & CSUM_DATA_VALID)
366 					n->m_pkthdr.csum_data = 0xffff;
367 				(void)if_simloop(ifp, n, dst->sa_family, hlen);
368 			} else
369 				ifp->if_iqdrops++;
370 		} else if (bcmp(eh->ether_dhost, eh->ether_shost,
371 				ETHER_ADDR_LEN) == 0) {
372 			m->m_pkthdr.csum_flags |= csum_flags;
373 			if (csum_flags & CSUM_DATA_VALID)
374 				m->m_pkthdr.csum_data = 0xffff;
375 			(void) if_simloop(ifp, m, dst->sa_family, hlen);
376 			return (0);	/* XXX */
377 		}
378 	}
379 
380        /*
381 	* Bridges require special output handling.
382 	*/
383 	if (ifp->if_bridge) {
384 		BRIDGE_OUTPUT(ifp, m, error);
385 		return (error);
386 	}
387 
388 #ifdef DEV_CARP
389 	if (ifp->if_carp &&
390 	    (error = carp_output(ifp, m, dst, NULL)))
391 		goto bad;
392 #endif
393 
394 	/* Handle ng_ether(4) processing, if any */
395 	if (IFP2AC(ifp)->ac_netgraph != NULL) {
396 		KASSERT(ng_ether_output_p != NULL,
397 		    ("ng_ether_output_p is NULL"));
398 		if ((error = (*ng_ether_output_p)(ifp, &m)) != 0) {
399 bad:			if (m != NULL)
400 				m_freem(m);
401 			return (error);
402 		}
403 		if (m == NULL)
404 			return (0);
405 	}
406 
407 	/* Continue with link-layer output */
408 	return ether_output_frame(ifp, m);
409 }
410 
411 /*
412  * Ethernet link layer output routine to send a raw frame to the device.
413  *
414  * This assumes that the 14 byte Ethernet header is present and contiguous
415  * in the first mbuf (if BRIDGE'ing).
416  */
417 int
418 ether_output_frame(struct ifnet *ifp, struct mbuf *m)
419 {
420 #if defined(INET) || defined(INET6)
421 	INIT_VNET_NET(ifp->if_vnet);
422 	struct ip_fw *rule = ip_dn_claim_rule(m);
423 
424 	if (IPFW_LOADED && V_ether_ipfw != 0) {
425 		if (ether_ipfw_chk(&m, ifp, &rule, 0) == 0) {
426 			if (m) {
427 				m_freem(m);
428 				return EACCES;	/* pkt dropped */
429 			} else
430 				return 0;	/* consumed e.g. in a pipe */
431 		}
432 	}
433 #endif
434 
435 	/*
436 	 * Queue message on interface, update output statistics if
437 	 * successful, and start output if interface not yet active.
438 	 */
439 	return ((ifp->if_transmit)(ifp, m));
440 }
441 
442 #if defined(INET) || defined(INET6)
443 /*
444  * ipfw processing for ethernet packets (in and out).
445  * The second parameter is NULL from ether_demux, and ifp from
446  * ether_output_frame.
447  */
448 int
449 ether_ipfw_chk(struct mbuf **m0, struct ifnet *dst,
450 	struct ip_fw **rule, int shared)
451 {
452 	INIT_VNET_INET(dst->if_vnet);
453 	struct ether_header *eh;
454 	struct ether_header save_eh;
455 	struct mbuf *m;
456 	int i;
457 	struct ip_fw_args args;
458 
459 	if (*rule != NULL && V_fw_one_pass)
460 		return 1; /* dummynet packet, already partially processed */
461 
462 	/*
463 	 * I need some amt of data to be contiguous, and in case others need
464 	 * the packet (shared==1) also better be in the first mbuf.
465 	 */
466 	m = *m0;
467 	i = min( m->m_pkthdr.len, max_protohdr);
468 	if ( shared || m->m_len < i) {
469 		m = m_pullup(m, i);
470 		if (m == NULL) {
471 			*m0 = m;
472 			return 0;
473 		}
474 	}
475 	eh = mtod(m, struct ether_header *);
476 	save_eh = *eh;			/* save copy for restore below */
477 	m_adj(m, ETHER_HDR_LEN);	/* strip ethernet header */
478 
479 	args.m = m;		/* the packet we are looking at		*/
480 	args.oif = dst;		/* destination, if any			*/
481 	args.rule = *rule;	/* matching rule to restart		*/
482 	args.next_hop = NULL;	/* we do not support forward yet	*/
483 	args.eh = &save_eh;	/* MAC header for bridged/MAC packets	*/
484 	args.inp = NULL;	/* used by ipfw uid/gid/jail rules	*/
485 	i = ip_fw_chk_ptr(&args);
486 	m = args.m;
487 	if (m != NULL) {
488 		/*
489 		 * Restore Ethernet header, as needed, in case the
490 		 * mbuf chain was replaced by ipfw.
491 		 */
492 		M_PREPEND(m, ETHER_HDR_LEN, M_DONTWAIT);
493 		if (m == NULL) {
494 			*m0 = m;
495 			return 0;
496 		}
497 		if (eh != mtod(m, struct ether_header *))
498 			bcopy(&save_eh, mtod(m, struct ether_header *),
499 				ETHER_HDR_LEN);
500 	}
501 	*m0 = m;
502 	*rule = args.rule;
503 
504 	if (i == IP_FW_DENY) /* drop */
505 		return 0;
506 
507 	KASSERT(m != NULL, ("ether_ipfw_chk: m is NULL"));
508 
509 	if (i == IP_FW_PASS) /* a PASS rule.  */
510 		return 1;
511 
512 	if (DUMMYNET_LOADED && (i == IP_FW_DUMMYNET)) {
513 		/*
514 		 * Pass the pkt to dummynet, which consumes it.
515 		 * If shared, make a copy and keep the original.
516 		 */
517 		if (shared) {
518 			m = m_copypacket(m, M_DONTWAIT);
519 			if (m == NULL)
520 				return 0;
521 		} else {
522 			/*
523 			 * Pass the original to dummynet and
524 			 * nothing back to the caller
525 			 */
526 			*m0 = NULL ;
527 		}
528 		ip_dn_io_ptr(&m, dst ? DN_TO_ETH_OUT: DN_TO_ETH_DEMUX, &args);
529 		return 0;
530 	}
531 	/*
532 	 * XXX at some point add support for divert/forward actions.
533 	 * If none of the above matches, we have to drop the pkt.
534 	 */
535 	return 0;
536 }
537 #endif
538 
539 /*
540  * Process a received Ethernet packet; the packet is in the
541  * mbuf chain m with the ethernet header at the front.
542  */
543 static void
544 ether_input(struct ifnet *ifp, struct mbuf *m)
545 {
546 	struct ether_header *eh;
547 	u_short etype;
548 
549 	if ((ifp->if_flags & IFF_UP) == 0) {
550 		m_freem(m);
551 		return;
552 	}
553 #ifdef DIAGNOSTIC
554 	if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0) {
555 		if_printf(ifp, "discard frame at !IFF_DRV_RUNNING\n");
556 		m_freem(m);
557 		return;
558 	}
559 #endif
560 	/*
561 	 * Do consistency checks to verify assumptions
562 	 * made by code past this point.
563 	 */
564 	if ((m->m_flags & M_PKTHDR) == 0) {
565 		if_printf(ifp, "discard frame w/o packet header\n");
566 		ifp->if_ierrors++;
567 		m_freem(m);
568 		return;
569 	}
570 	if (m->m_len < ETHER_HDR_LEN) {
571 		/* XXX maybe should pullup? */
572 		if_printf(ifp, "discard frame w/o leading ethernet "
573 				"header (len %u pkt len %u)\n",
574 				m->m_len, m->m_pkthdr.len);
575 		ifp->if_ierrors++;
576 		m_freem(m);
577 		return;
578 	}
579 	eh = mtod(m, struct ether_header *);
580 	etype = ntohs(eh->ether_type);
581 	if (m->m_pkthdr.rcvif == NULL) {
582 		if_printf(ifp, "discard frame w/o interface pointer\n");
583 		ifp->if_ierrors++;
584 		m_freem(m);
585 		return;
586 	}
587 #ifdef DIAGNOSTIC
588 	if (m->m_pkthdr.rcvif != ifp) {
589 		if_printf(ifp, "Warning, frame marked as received on %s\n",
590 			m->m_pkthdr.rcvif->if_xname);
591 	}
592 #endif
593 
594 	if (ETHER_IS_MULTICAST(eh->ether_dhost)) {
595 		if (ETHER_IS_BROADCAST(eh->ether_dhost))
596 			m->m_flags |= M_BCAST;
597 		else
598 			m->m_flags |= M_MCAST;
599 		ifp->if_imcasts++;
600 	}
601 
602 #ifdef MAC
603 	/*
604 	 * Tag the mbuf with an appropriate MAC label before any other
605 	 * consumers can get to it.
606 	 */
607 	mac_ifnet_create_mbuf(ifp, m);
608 #endif
609 
610 	/*
611 	 * Give bpf a chance at the packet.
612 	 */
613 	ETHER_BPF_MTAP(ifp, m);
614 
615 	/*
616 	 * If the CRC is still on the packet, trim it off. We do this once
617 	 * and once only in case we are re-entered. Nothing else on the
618 	 * Ethernet receive path expects to see the FCS.
619 	 */
620 	if (m->m_flags & M_HASFCS) {
621 		m_adj(m, -ETHER_CRC_LEN);
622 		m->m_flags &= ~M_HASFCS;
623 	}
624 
625 	ifp->if_ibytes += m->m_pkthdr.len;
626 
627 	/* Allow monitor mode to claim this frame, after stats are updated. */
628 	if (ifp->if_flags & IFF_MONITOR) {
629 		m_freem(m);
630 		return;
631 	}
632 
633 	/* Handle input from a lagg(4) port */
634 	if (ifp->if_type == IFT_IEEE8023ADLAG) {
635 		KASSERT(lagg_input_p != NULL,
636 		    ("%s: if_lagg not loaded!", __func__));
637 		m = (*lagg_input_p)(ifp, m);
638 		if (m != NULL)
639 			ifp = m->m_pkthdr.rcvif;
640 		else
641 			return;
642 	}
643 
644 	/*
645 	 * If the hardware did not process an 802.1Q tag, do this now,
646 	 * to allow 802.1P priority frames to be passed to the main input
647 	 * path correctly.
648 	 * TODO: Deal with Q-in-Q frames, but not arbitrary nesting levels.
649 	 */
650 	if ((m->m_flags & M_VLANTAG) == 0 && etype == ETHERTYPE_VLAN) {
651 		struct ether_vlan_header *evl;
652 
653 		if (m->m_len < sizeof(*evl) &&
654 		    (m = m_pullup(m, sizeof(*evl))) == NULL) {
655 #ifdef DIAGNOSTIC
656 			if_printf(ifp, "cannot pullup VLAN header\n");
657 #endif
658 			ifp->if_ierrors++;
659 			m_freem(m);
660 			return;
661 		}
662 
663 		evl = mtod(m, struct ether_vlan_header *);
664 		m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
665 		m->m_flags |= M_VLANTAG;
666 
667 		bcopy((char *)evl, (char *)evl + ETHER_VLAN_ENCAP_LEN,
668 		    ETHER_HDR_LEN - ETHER_TYPE_LEN);
669 		m_adj(m, ETHER_VLAN_ENCAP_LEN);
670 	}
671 
672 	/* Allow ng_ether(4) to claim this frame. */
673 	if (IFP2AC(ifp)->ac_netgraph != NULL) {
674 		KASSERT(ng_ether_input_p != NULL,
675 		    ("%s: ng_ether_input_p is NULL", __func__));
676 		m->m_flags &= ~M_PROMISC;
677 		(*ng_ether_input_p)(ifp, &m);
678 		if (m == NULL)
679 			return;
680 	}
681 
682 	/*
683 	 * Allow if_bridge(4) to claim this frame.
684 	 * The BRIDGE_INPUT() macro will update ifp if the bridge changed it
685 	 * and the frame should be delivered locally.
686 	 */
687 	if (ifp->if_bridge != NULL) {
688 		m->m_flags &= ~M_PROMISC;
689 		BRIDGE_INPUT(ifp, m);
690 		if (m == NULL)
691 			return;
692 	}
693 
694 #ifdef DEV_CARP
695 	/*
696 	 * Clear M_PROMISC on frame so that carp(4) will see it when the
697 	 * mbuf flows up to Layer 3.
698 	 * FreeBSD's implementation of carp(4) uses the inprotosw
699 	 * to dispatch IPPROTO_CARP. carp(4) also allocates its own
700 	 * Ethernet addresses of the form 00:00:5e:00:01:xx, which
701 	 * is outside the scope of the M_PROMISC test below.
702 	 * TODO: Maintain a hash table of ethernet addresses other than
703 	 * ether_dhost which may be active on this ifp.
704 	 */
705 	if (ifp->if_carp && carp_forus(ifp->if_carp, eh->ether_dhost)) {
706 		m->m_flags &= ~M_PROMISC;
707 	} else
708 #endif
709 	{
710 		/*
711 		 * If the frame received was not for our MAC address, set the
712 		 * M_PROMISC flag on the mbuf chain. The frame may need to
713 		 * be seen by the rest of the Ethernet input path in case of
714 		 * re-entry (e.g. bridge, vlan, netgraph) but should not be
715 		 * seen by upper protocol layers.
716 		 */
717 		if (!ETHER_IS_MULTICAST(eh->ether_dhost) &&
718 		    bcmp(IF_LLADDR(ifp), eh->ether_dhost, ETHER_ADDR_LEN) != 0)
719 			m->m_flags |= M_PROMISC;
720 	}
721 
722 	/* First chunk of an mbuf contains good entropy */
723 	if (harvest.ethernet)
724 		random_harvest(m, 16, 3, 0, RANDOM_NET);
725 
726 	ether_demux(ifp, m);
727 }
728 
729 /*
730  * Upper layer processing for a received Ethernet packet.
731  */
732 void
733 ether_demux(struct ifnet *ifp, struct mbuf *m)
734 {
735 	struct ether_header *eh;
736 	int isr;
737 	u_short ether_type;
738 #if defined(NETATALK)
739 	struct llc *l;
740 #endif
741 
742 	KASSERT(ifp != NULL, ("%s: NULL interface pointer", __func__));
743 
744 #if defined(INET) || defined(INET6)
745 	INIT_VNET_NET(ifp->if_vnet);
746 	/*
747 	 * Allow dummynet and/or ipfw to claim the frame.
748 	 * Do not do this for PROMISC frames in case we are re-entered.
749 	 */
750 	if (IPFW_LOADED && V_ether_ipfw != 0 && !(m->m_flags & M_PROMISC)) {
751 		struct ip_fw *rule = ip_dn_claim_rule(m);
752 
753 		if (ether_ipfw_chk(&m, NULL, &rule, 0) == 0) {
754 			if (m)
755 				m_freem(m);	/* dropped; free mbuf chain */
756 			return;			/* consumed */
757 		}
758 	}
759 #endif
760 	eh = mtod(m, struct ether_header *);
761 	ether_type = ntohs(eh->ether_type);
762 
763 	/*
764 	 * If this frame has a VLAN tag other than 0, call vlan_input()
765 	 * if its module is loaded. Otherwise, drop.
766 	 */
767 	if ((m->m_flags & M_VLANTAG) &&
768 	    EVL_VLANOFTAG(m->m_pkthdr.ether_vtag) != 0) {
769 		if (ifp->if_vlantrunk == NULL) {
770 			ifp->if_noproto++;
771 			m_freem(m);
772 			return;
773 		}
774 		KASSERT(vlan_input_p != NULL,("%s: VLAN not loaded!",
775 		    __func__));
776 		/* Clear before possibly re-entering ether_input(). */
777 		m->m_flags &= ~M_PROMISC;
778 		(*vlan_input_p)(ifp, m);
779 		return;
780 	}
781 
782 	/*
783 	 * Pass promiscuously received frames to the upper layer if the user
784 	 * requested this by setting IFF_PPROMISC. Otherwise, drop them.
785 	 */
786 	if ((ifp->if_flags & IFF_PPROMISC) == 0 && (m->m_flags & M_PROMISC)) {
787 		m_freem(m);
788 		return;
789 	}
790 
791 	/*
792 	 * Reset layer specific mbuf flags to avoid confusing upper layers.
793 	 * Strip off Ethernet header.
794 	 */
795 	m->m_flags &= ~M_VLANTAG;
796 	m->m_flags &= ~(M_PROTOFLAGS);
797 	m_adj(m, ETHER_HDR_LEN);
798 
799 	/*
800 	 * Dispatch frame to upper layer.
801 	 */
802 	switch (ether_type) {
803 #ifdef INET
804 	case ETHERTYPE_IP:
805 		if ((m = ip_fastforward(m)) == NULL)
806 			return;
807 		isr = NETISR_IP;
808 		break;
809 
810 	case ETHERTYPE_ARP:
811 		if (ifp->if_flags & IFF_NOARP) {
812 			/* Discard packet if ARP is disabled on interface */
813 			m_freem(m);
814 			return;
815 		}
816 		isr = NETISR_ARP;
817 		break;
818 #endif
819 #ifdef IPX
820 	case ETHERTYPE_IPX:
821 		if (ef_inputp && ef_inputp(ifp, eh, m) == 0)
822 			return;
823 		isr = NETISR_IPX;
824 		break;
825 #endif
826 #ifdef INET6
827 	case ETHERTYPE_IPV6:
828 		isr = NETISR_IPV6;
829 		break;
830 #endif
831 #ifdef NETATALK
832 	case ETHERTYPE_AT:
833 		isr = NETISR_ATALK1;
834 		break;
835 	case ETHERTYPE_AARP:
836 		isr = NETISR_AARP;
837 		break;
838 #endif /* NETATALK */
839 	default:
840 #ifdef IPX
841 		if (ef_inputp && ef_inputp(ifp, eh, m) == 0)
842 			return;
843 #endif /* IPX */
844 #if defined(NETATALK)
845 		if (ether_type > ETHERMTU)
846 			goto discard;
847 		l = mtod(m, struct llc *);
848 		if (l->llc_dsap == LLC_SNAP_LSAP &&
849 		    l->llc_ssap == LLC_SNAP_LSAP &&
850 		    l->llc_control == LLC_UI) {
851 			if (bcmp(&(l->llc_snap_org_code)[0], at_org_code,
852 			    sizeof(at_org_code)) == 0 &&
853 			    ntohs(l->llc_snap_ether_type) == ETHERTYPE_AT) {
854 				m_adj(m, LLC_SNAPFRAMELEN);
855 				isr = NETISR_ATALK2;
856 				break;
857 			}
858 			if (bcmp(&(l->llc_snap_org_code)[0], aarp_org_code,
859 			    sizeof(aarp_org_code)) == 0 &&
860 			    ntohs(l->llc_snap_ether_type) == ETHERTYPE_AARP) {
861 				m_adj(m, LLC_SNAPFRAMELEN);
862 				isr = NETISR_AARP;
863 				break;
864 			}
865 		}
866 #endif /* NETATALK */
867 		goto discard;
868 	}
869 	netisr_dispatch(isr, m);
870 	return;
871 
872 discard:
873 	/*
874 	 * Packet is to be discarded.  If netgraph is present,
875 	 * hand the packet to it for last chance processing;
876 	 * otherwise dispose of it.
877 	 */
878 	if (IFP2AC(ifp)->ac_netgraph != NULL) {
879 		KASSERT(ng_ether_input_orphan_p != NULL,
880 		    ("ng_ether_input_orphan_p is NULL"));
881 		/*
882 		 * Put back the ethernet header so netgraph has a
883 		 * consistent view of inbound packets.
884 		 */
885 		M_PREPEND(m, ETHER_HDR_LEN, M_DONTWAIT);
886 		(*ng_ether_input_orphan_p)(ifp, m);
887 		return;
888 	}
889 	m_freem(m);
890 }
891 
892 /*
893  * Convert Ethernet address to printable (loggable) representation.
894  * This routine is for compatibility; it's better to just use
895  *
896  *	printf("%6D", <pointer to address>, ":");
897  *
898  * since there's no static buffer involved.
899  */
900 char *
901 ether_sprintf(const u_char *ap)
902 {
903 	static char etherbuf[18];
904 	snprintf(etherbuf, sizeof (etherbuf), "%6D", ap, ":");
905 	return (etherbuf);
906 }
907 
908 /*
909  * Perform common duties while attaching to interface list
910  */
911 void
912 ether_ifattach(struct ifnet *ifp, const u_int8_t *lla)
913 {
914 	int i;
915 	struct ifaddr *ifa;
916 	struct sockaddr_dl *sdl;
917 
918 	ifp->if_addrlen = ETHER_ADDR_LEN;
919 	ifp->if_hdrlen = ETHER_HDR_LEN;
920 	if_attach(ifp);
921 	ifp->if_mtu = ETHERMTU;
922 	ifp->if_output = ether_output;
923 	ifp->if_input = ether_input;
924 	ifp->if_resolvemulti = ether_resolvemulti;
925 	if (ifp->if_baudrate == 0)
926 		ifp->if_baudrate = IF_Mbps(10);		/* just a default */
927 	ifp->if_broadcastaddr = etherbroadcastaddr;
928 
929 	ifa = ifp->if_addr;
930 	KASSERT(ifa != NULL, ("%s: no lladdr!\n", __func__));
931 	sdl = (struct sockaddr_dl *)ifa->ifa_addr;
932 	sdl->sdl_type = IFT_ETHER;
933 	sdl->sdl_alen = ifp->if_addrlen;
934 	bcopy(lla, LLADDR(sdl), ifp->if_addrlen);
935 
936 	bpfattach(ifp, DLT_EN10MB, ETHER_HDR_LEN);
937 	if (ng_ether_attach_p != NULL)
938 		(*ng_ether_attach_p)(ifp);
939 
940 	/* Announce Ethernet MAC address if non-zero. */
941 	for (i = 0; i < ifp->if_addrlen; i++)
942 		if (lla[i] != 0)
943 			break;
944 	if (i != ifp->if_addrlen)
945 		if_printf(ifp, "Ethernet address: %6D\n", lla, ":");
946 }
947 
948 /*
949  * Perform common duties while detaching an Ethernet interface
950  */
951 void
952 ether_ifdetach(struct ifnet *ifp)
953 {
954 	if (IFP2AC(ifp)->ac_netgraph != NULL) {
955 		KASSERT(ng_ether_detach_p != NULL,
956 		    ("ng_ether_detach_p is NULL"));
957 		(*ng_ether_detach_p)(ifp);
958 	}
959 
960 	bpfdetach(ifp);
961 	if_detach(ifp);
962 }
963 
964 SYSCTL_DECL(_net_link);
965 SYSCTL_NODE(_net_link, IFT_ETHER, ether, CTLFLAG_RW, 0, "Ethernet");
966 #if defined(INET) || defined(INET6)
967 SYSCTL_V_INT(V_NET, vnet_net, _net_link_ether, OID_AUTO, ipfw, CTLFLAG_RW,
968 	     ether_ipfw, 0, "Pass ether pkts through firewall");
969 #endif
970 
971 #if 0
972 /*
973  * This is for reference.  We have a table-driven version
974  * of the little-endian crc32 generator, which is faster
975  * than the double-loop.
976  */
977 uint32_t
978 ether_crc32_le(const uint8_t *buf, size_t len)
979 {
980 	size_t i;
981 	uint32_t crc;
982 	int bit;
983 	uint8_t data;
984 
985 	crc = 0xffffffff;	/* initial value */
986 
987 	for (i = 0; i < len; i++) {
988 		for (data = *buf++, bit = 0; bit < 8; bit++, data >>= 1) {
989 			carry = (crc ^ data) & 1;
990 			crc >>= 1;
991 			if (carry)
992 				crc = (crc ^ ETHER_CRC_POLY_LE);
993 		}
994 	}
995 
996 	return (crc);
997 }
998 #else
999 uint32_t
1000 ether_crc32_le(const uint8_t *buf, size_t len)
1001 {
1002 	static const uint32_t crctab[] = {
1003 		0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac,
1004 		0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c,
1005 		0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c,
1006 		0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c
1007 	};
1008 	size_t i;
1009 	uint32_t crc;
1010 
1011 	crc = 0xffffffff;	/* initial value */
1012 
1013 	for (i = 0; i < len; i++) {
1014 		crc ^= buf[i];
1015 		crc = (crc >> 4) ^ crctab[crc & 0xf];
1016 		crc = (crc >> 4) ^ crctab[crc & 0xf];
1017 	}
1018 
1019 	return (crc);
1020 }
1021 #endif
1022 
1023 uint32_t
1024 ether_crc32_be(const uint8_t *buf, size_t len)
1025 {
1026 	size_t i;
1027 	uint32_t crc, carry;
1028 	int bit;
1029 	uint8_t data;
1030 
1031 	crc = 0xffffffff;	/* initial value */
1032 
1033 	for (i = 0; i < len; i++) {
1034 		for (data = *buf++, bit = 0; bit < 8; bit++, data >>= 1) {
1035 			carry = ((crc & 0x80000000) ? 1 : 0) ^ (data & 0x01);
1036 			crc <<= 1;
1037 			if (carry)
1038 				crc = (crc ^ ETHER_CRC_POLY_BE) | carry;
1039 		}
1040 	}
1041 
1042 	return (crc);
1043 }
1044 
1045 int
1046 ether_ioctl(struct ifnet *ifp, u_long command, caddr_t data)
1047 {
1048 	struct ifaddr *ifa = (struct ifaddr *) data;
1049 	struct ifreq *ifr = (struct ifreq *) data;
1050 	int error = 0;
1051 
1052 	switch (command) {
1053 	case SIOCSIFADDR:
1054 		ifp->if_flags |= IFF_UP;
1055 
1056 		switch (ifa->ifa_addr->sa_family) {
1057 #ifdef INET
1058 		case AF_INET:
1059 			ifp->if_init(ifp->if_softc);	/* before arpwhohas */
1060 			arp_ifinit(ifp, ifa);
1061 			break;
1062 #endif
1063 #ifdef IPX
1064 		/*
1065 		 * XXX - This code is probably wrong
1066 		 */
1067 		case AF_IPX:
1068 			{
1069 			struct ipx_addr *ina = &(IA_SIPX(ifa)->sipx_addr);
1070 
1071 			if (ipx_nullhost(*ina))
1072 				ina->x_host =
1073 				    *(union ipx_host *)
1074 				    IF_LLADDR(ifp);
1075 			else {
1076 				bcopy((caddr_t) ina->x_host.c_host,
1077 				      (caddr_t) IF_LLADDR(ifp),
1078 				      ETHER_ADDR_LEN);
1079 			}
1080 
1081 			/*
1082 			 * Set new address
1083 			 */
1084 			ifp->if_init(ifp->if_softc);
1085 			break;
1086 			}
1087 #endif
1088 		default:
1089 			ifp->if_init(ifp->if_softc);
1090 			break;
1091 		}
1092 		break;
1093 
1094 	case SIOCGIFADDR:
1095 		{
1096 			struct sockaddr *sa;
1097 
1098 			sa = (struct sockaddr *) & ifr->ifr_data;
1099 			bcopy(IF_LLADDR(ifp),
1100 			      (caddr_t) sa->sa_data, ETHER_ADDR_LEN);
1101 		}
1102 		break;
1103 
1104 	case SIOCSIFMTU:
1105 		/*
1106 		 * Set the interface MTU.
1107 		 */
1108 		if (ifr->ifr_mtu > ETHERMTU) {
1109 			error = EINVAL;
1110 		} else {
1111 			ifp->if_mtu = ifr->ifr_mtu;
1112 		}
1113 		break;
1114 	default:
1115 		error = EINVAL;			/* XXX netbsd has ENOTTY??? */
1116 		break;
1117 	}
1118 	return (error);
1119 }
1120 
1121 static int
1122 ether_resolvemulti(struct ifnet *ifp, struct sockaddr **llsa,
1123 	struct sockaddr *sa)
1124 {
1125 	struct sockaddr_dl *sdl;
1126 #ifdef INET
1127 	struct sockaddr_in *sin;
1128 #endif
1129 #ifdef INET6
1130 	struct sockaddr_in6 *sin6;
1131 #endif
1132 	u_char *e_addr;
1133 
1134 	switch(sa->sa_family) {
1135 	case AF_LINK:
1136 		/*
1137 		 * No mapping needed. Just check that it's a valid MC address.
1138 		 */
1139 		sdl = (struct sockaddr_dl *)sa;
1140 		e_addr = LLADDR(sdl);
1141 		if (!ETHER_IS_MULTICAST(e_addr))
1142 			return EADDRNOTAVAIL;
1143 		*llsa = 0;
1144 		return 0;
1145 
1146 #ifdef INET
1147 	case AF_INET:
1148 		sin = (struct sockaddr_in *)sa;
1149 		if (!IN_MULTICAST(ntohl(sin->sin_addr.s_addr)))
1150 			return EADDRNOTAVAIL;
1151 		sdl = malloc(sizeof *sdl, M_IFMADDR,
1152 		       M_NOWAIT|M_ZERO);
1153 		if (sdl == NULL)
1154 			return ENOMEM;
1155 		sdl->sdl_len = sizeof *sdl;
1156 		sdl->sdl_family = AF_LINK;
1157 		sdl->sdl_index = ifp->if_index;
1158 		sdl->sdl_type = IFT_ETHER;
1159 		sdl->sdl_alen = ETHER_ADDR_LEN;
1160 		e_addr = LLADDR(sdl);
1161 		ETHER_MAP_IP_MULTICAST(&sin->sin_addr, e_addr);
1162 		*llsa = (struct sockaddr *)sdl;
1163 		return 0;
1164 #endif
1165 #ifdef INET6
1166 	case AF_INET6:
1167 		sin6 = (struct sockaddr_in6 *)sa;
1168 		if (IN6_IS_ADDR_UNSPECIFIED(&sin6->sin6_addr)) {
1169 			/*
1170 			 * An IP6 address of 0 means listen to all
1171 			 * of the Ethernet multicast address used for IP6.
1172 			 * (This is used for multicast routers.)
1173 			 */
1174 			ifp->if_flags |= IFF_ALLMULTI;
1175 			*llsa = 0;
1176 			return 0;
1177 		}
1178 		if (!IN6_IS_ADDR_MULTICAST(&sin6->sin6_addr))
1179 			return EADDRNOTAVAIL;
1180 		sdl = malloc(sizeof *sdl, M_IFMADDR,
1181 		       M_NOWAIT|M_ZERO);
1182 		if (sdl == NULL)
1183 			return (ENOMEM);
1184 		sdl->sdl_len = sizeof *sdl;
1185 		sdl->sdl_family = AF_LINK;
1186 		sdl->sdl_index = ifp->if_index;
1187 		sdl->sdl_type = IFT_ETHER;
1188 		sdl->sdl_alen = ETHER_ADDR_LEN;
1189 		e_addr = LLADDR(sdl);
1190 		ETHER_MAP_IPV6_MULTICAST(&sin6->sin6_addr, e_addr);
1191 		*llsa = (struct sockaddr *)sdl;
1192 		return 0;
1193 #endif
1194 
1195 	default:
1196 		/*
1197 		 * Well, the text isn't quite right, but it's the name
1198 		 * that counts...
1199 		 */
1200 		return EAFNOSUPPORT;
1201 	}
1202 }
1203 
1204 static void*
1205 ether_alloc(u_char type, struct ifnet *ifp)
1206 {
1207 	struct arpcom	*ac;
1208 
1209 	ac = malloc(sizeof(struct arpcom), M_ARPCOM, M_WAITOK | M_ZERO);
1210 	ac->ac_ifp = ifp;
1211 
1212 	return (ac);
1213 }
1214 
1215 static void
1216 ether_free(void *com, u_char type)
1217 {
1218 
1219 	free(com, M_ARPCOM);
1220 }
1221 
1222 static int
1223 ether_modevent(module_t mod, int type, void *data)
1224 {
1225 
1226 	switch (type) {
1227 	case MOD_LOAD:
1228 		if_register_com_alloc(IFT_ETHER, ether_alloc, ether_free);
1229 		break;
1230 	case MOD_UNLOAD:
1231 		if_deregister_com_alloc(IFT_ETHER);
1232 		break;
1233 	default:
1234 		return EOPNOTSUPP;
1235 	}
1236 
1237 	return (0);
1238 }
1239 
1240 static moduledata_t ether_mod = {
1241 	"ether",
1242 	ether_modevent,
1243 	0
1244 };
1245 
1246 void
1247 ether_vlan_mtap(struct bpf_if *bp, struct mbuf *m, void *data, u_int dlen)
1248 {
1249 	struct ether_vlan_header vlan;
1250 	struct mbuf mv, mb;
1251 
1252 	KASSERT((m->m_flags & M_VLANTAG) != 0,
1253 	    ("%s: vlan information not present", __func__));
1254 	KASSERT(m->m_len >= sizeof(struct ether_header),
1255 	    ("%s: mbuf not large enough for header", __func__));
1256 	bcopy(mtod(m, char *), &vlan, sizeof(struct ether_header));
1257 	vlan.evl_proto = vlan.evl_encap_proto;
1258 	vlan.evl_encap_proto = htons(ETHERTYPE_VLAN);
1259 	vlan.evl_tag = htons(m->m_pkthdr.ether_vtag);
1260 	m->m_len -= sizeof(struct ether_header);
1261 	m->m_data += sizeof(struct ether_header);
1262 	/*
1263 	 * If a data link has been supplied by the caller, then we will need to
1264 	 * re-create a stack allocated mbuf chain with the following structure:
1265 	 *
1266 	 * (1) mbuf #1 will contain the supplied data link
1267 	 * (2) mbuf #2 will contain the vlan header
1268 	 * (3) mbuf #3 will contain the original mbuf's packet data
1269 	 *
1270 	 * Otherwise, submit the packet and vlan header via bpf_mtap2().
1271 	 */
1272 	if (data != NULL) {
1273 		mv.m_next = m;
1274 		mv.m_data = (caddr_t)&vlan;
1275 		mv.m_len = sizeof(vlan);
1276 		mb.m_next = &mv;
1277 		mb.m_data = data;
1278 		mb.m_len = dlen;
1279 		bpf_mtap(bp, &mb);
1280 	} else
1281 		bpf_mtap2(bp, &vlan, sizeof(vlan), m);
1282 	m->m_len += sizeof(struct ether_header);
1283 	m->m_data -= sizeof(struct ether_header);
1284 }
1285 
1286 struct mbuf *
1287 ether_vlanencap(struct mbuf *m, uint16_t tag)
1288 {
1289 	struct ether_vlan_header *evl;
1290 
1291 	M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
1292 	if (m == NULL)
1293 		return (NULL);
1294 	/* M_PREPEND takes care of m_len, m_pkthdr.len for us */
1295 
1296 	if (m->m_len < sizeof(*evl)) {
1297 		m = m_pullup(m, sizeof(*evl));
1298 		if (m == NULL)
1299 			return (NULL);
1300 	}
1301 
1302 	/*
1303 	 * Transform the Ethernet header into an Ethernet header
1304 	 * with 802.1Q encapsulation.
1305 	 */
1306 	evl = mtod(m, struct ether_vlan_header *);
1307 	bcopy((char *)evl + ETHER_VLAN_ENCAP_LEN,
1308 	    (char *)evl, ETHER_HDR_LEN - ETHER_TYPE_LEN);
1309 	evl->evl_encap_proto = htons(ETHERTYPE_VLAN);
1310 	evl->evl_tag = htons(tag);
1311 	return (m);
1312 }
1313 
1314 DECLARE_MODULE(ether, ether_mod, SI_SUB_INIT_IF, SI_ORDER_ANY);
1315 MODULE_VERSION(ether, 1);
1316