xref: /freebsd/sys/netgraph/ng_vlan.c (revision 66fd12cf4896eb08ad8e7a2627537f84ead84dd3)
1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause
3  *
4  * Copyright (c) 2003 IPNET Internet Communication Company
5  * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  * 1. Redistributions of source code must retain the above copyright
12  *    notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  *    notice, this list of conditions and the following disclaimer in the
15  *    documentation and/or other materials provided with the distribution.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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  * Author: Ruslan Ermilov <ru@FreeBSD.org>
30  *
31  * $FreeBSD$
32  */
33 
34 #include <sys/param.h>
35 #include <sys/errno.h>
36 #include <sys/kernel.h>
37 #include <sys/malloc.h>
38 #include <sys/mbuf.h>
39 #include <sys/queue.h>
40 #include <sys/socket.h>
41 #include <sys/systm.h>
42 
43 #include <net/ethernet.h>
44 #include <net/if.h>
45 #include <net/if_vlan_var.h>
46 
47 #include <netgraph/ng_message.h>
48 #include <netgraph/ng_parse.h>
49 #include <netgraph/ng_vlan.h>
50 #include <netgraph/netgraph.h>
51 
52 struct ng_vlan_private {
53 	hook_p		downstream_hook;
54 	hook_p		nomatch_hook;
55 	uint32_t	decap_enable;
56 	uint32_t	encap_enable;
57 	uint16_t	encap_proto;
58 	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];
59 };
60 typedef struct ng_vlan_private *priv_p;
61 
62 #define	ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)
63 #define	VLAN_TAG_MASK	0xFFFF
64 #define	HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
65 #define	IS_HOOK_VLAN_SET(hdata) \
66 	    ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) == HOOK_VLAN_TAG_SET_MASK)
67 
68 static ng_constructor_t	ng_vlan_constructor;
69 static ng_rcvmsg_t	ng_vlan_rcvmsg;
70 static ng_shutdown_t	ng_vlan_shutdown;
71 static ng_newhook_t	ng_vlan_newhook;
72 static ng_rcvdata_t	ng_vlan_rcvdata;
73 static ng_disconnect_t	ng_vlan_disconnect;
74 
75 /* Parse type for struct ng_vlan_filter. */
76 static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
77 	NG_VLAN_FILTER_FIELDS;
78 static const struct ng_parse_type ng_vlan_filter_type = {
79 	&ng_parse_struct_type,
80 	&ng_vlan_filter_fields
81 };
82 
83 static int
84 ng_vlan_getTableLength(const struct ng_parse_type *type,
85     const u_char *start, const u_char *buf)
86 {
87 	const struct ng_vlan_table *const table =
88 	    (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
89 
90 	return table->n;
91 }
92 
93 /* Parse type for struct ng_vlan_table. */
94 static const struct ng_parse_array_info ng_vlan_table_array_info = {
95 	&ng_vlan_filter_type,
96 	ng_vlan_getTableLength
97 };
98 static const struct ng_parse_type ng_vlan_table_array_type = {
99 	&ng_parse_array_type,
100 	&ng_vlan_table_array_info
101 };
102 static const struct ng_parse_struct_field ng_vlan_table_fields[] =
103 	NG_VLAN_TABLE_FIELDS;
104 static const struct ng_parse_type ng_vlan_table_type = {
105 	&ng_parse_struct_type,
106 	&ng_vlan_table_fields
107 };
108 
109 /* List of commands and how to convert arguments to/from ASCII. */
110 static const struct ng_cmdlist ng_vlan_cmdlist[] = {
111 	{
112 	  NGM_VLAN_COOKIE,
113 	  NGM_VLAN_ADD_FILTER,
114 	  "addfilter",
115 	  &ng_vlan_filter_type,
116 	  NULL
117 	},
118 	{
119 	  NGM_VLAN_COOKIE,
120 	  NGM_VLAN_DEL_FILTER,
121 	  "delfilter",
122 	  &ng_parse_hookbuf_type,
123 	  NULL
124 	},
125 	{
126 	  NGM_VLAN_COOKIE,
127 	  NGM_VLAN_GET_TABLE,
128 	  "gettable",
129 	  NULL,
130 	  &ng_vlan_table_type
131 	},
132 	{
133 	  NGM_VLAN_COOKIE,
134 	  NGM_VLAN_DEL_VID_FLT,
135 	  "delvidflt",
136 	  &ng_parse_uint16_type,
137 	  NULL
138 	},
139 	{
140 	  NGM_VLAN_COOKIE,
141 	  NGM_VLAN_GET_DECAP,
142 	  "getdecap",
143 	  NULL,
144 	  &ng_parse_hint32_type
145 	},
146 	{
147 	  NGM_VLAN_COOKIE,
148 	  NGM_VLAN_SET_DECAP,
149 	  "setdecap",
150 	  &ng_parse_hint32_type,
151 	  NULL
152 	},
153 	{
154 	  NGM_VLAN_COOKIE,
155 	  NGM_VLAN_GET_ENCAP,
156 	  "getencap",
157 	  NULL,
158 	  &ng_parse_hint32_type
159 	},
160 	{
161 	  NGM_VLAN_COOKIE,
162 	  NGM_VLAN_SET_ENCAP,
163 	  "setencap",
164 	  &ng_parse_hint32_type,
165 	  NULL
166 	},
167 	{
168 	  NGM_VLAN_COOKIE,
169 	  NGM_VLAN_GET_ENCAP_PROTO,
170 	  "getencapproto",
171 	  NULL,
172 	  &ng_parse_hint16_type
173 	},
174 	{
175 	  NGM_VLAN_COOKIE,
176 	  NGM_VLAN_SET_ENCAP_PROTO,
177 	  "setencapproto",
178 	  &ng_parse_hint16_type,
179 	  NULL
180 	},
181 	{ 0 }
182 };
183 
184 static struct ng_type ng_vlan_typestruct = {
185 	.version =	NG_ABI_VERSION,
186 	.name =		NG_VLAN_NODE_TYPE,
187 	.constructor =	ng_vlan_constructor,
188 	.rcvmsg =	ng_vlan_rcvmsg,
189 	.shutdown =	ng_vlan_shutdown,
190 	.newhook =	ng_vlan_newhook,
191 	.rcvdata =	ng_vlan_rcvdata,
192 	.disconnect =	ng_vlan_disconnect,
193 	.cmdlist =	ng_vlan_cmdlist,
194 };
195 NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
196 
197 /*
198  * Helper functions.
199  */
200 
201 static __inline int
202 m_chk(struct mbuf **mp, int len)
203 {
204 
205 	if ((*mp)->m_pkthdr.len < len) {
206 		m_freem((*mp));
207 		(*mp) = NULL;
208 		return (EINVAL);
209 	}
210 	if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
211 		return (ENOBUFS);
212 
213 	return (0);
214 }
215 
216 /*
217  * Netgraph node functions.
218  */
219 
220 static int
221 ng_vlan_constructor(node_p node)
222 {
223 	priv_p priv;
224 
225 	priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
226 	priv->decap_enable = 0;
227 	priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
228 	priv->encap_proto = htons(ETHERTYPE_VLAN);
229 	NG_NODE_SET_PRIVATE(node, priv);
230 	return (0);
231 }
232 
233 static int
234 ng_vlan_newhook(node_p node, hook_p hook, const char *name)
235 {
236 	const priv_p priv = NG_NODE_PRIVATE(node);
237 
238 	if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
239 		priv->downstream_hook = hook;
240 	else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
241 		priv->nomatch_hook = hook;
242 	else {
243 		/*
244 		 * Any other hook name is valid and can
245 		 * later be associated with a filter rule.
246 		 */
247 	}
248 	NG_HOOK_SET_PRIVATE(hook, NULL);
249 	return (0);
250 }
251 
252 static int
253 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
254 {
255 	const priv_p priv = NG_NODE_PRIVATE(node);
256 	struct ng_mesg *msg, *resp = NULL;
257 	struct ng_vlan_filter *vf;
258 	hook_p hook;
259 	struct ng_vlan_table *t;
260 	uintptr_t hook_data;
261 	int i, vlan_count;
262 	uint16_t vid;
263 	int error = 0;
264 
265 	NGI_GET_MSG(item, msg);
266 	/* Deal with message according to cookie and command. */
267 	switch (msg->header.typecookie) {
268 	case NGM_VLAN_COOKIE:
269 		switch (msg->header.cmd) {
270 		case NGM_VLAN_ADD_FILTER:
271 			/* Check that message is long enough. */
272 			if (msg->header.arglen != sizeof(*vf)) {
273 				error = EINVAL;
274 				break;
275 			}
276 			vf = (struct ng_vlan_filter *)msg->data;
277 			/* Sanity check the VLAN ID value. */
278 #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
279 			if (vf->vid == 0 && vf->vid != vf->vlan) {
280 				vf->vid = vf->vlan;
281 			} else if (vf->vid != 0 && vf->vlan != 0 &&
282 			    vf->vid != vf->vlan) {
283 				error = EINVAL;
284 				break;
285 			}
286 #endif
287 			if (vf->vid & ~EVL_VLID_MASK ||
288 			    vf->pcp & ~7 ||
289 			    vf->cfi & ~1) {
290 				error = EINVAL;
291 				break;
292 			}
293 			/* Check that a referenced hook exists. */
294 			hook = ng_findhook(node, vf->hook_name);
295 			if (hook == NULL) {
296 				error = ENOENT;
297 				break;
298 			}
299 			/* And is not one of the special hooks. */
300 			if (hook == priv->downstream_hook ||
301 			    hook == priv->nomatch_hook) {
302 				error = EINVAL;
303 				break;
304 			}
305 			/* And is not already in service. */
306 			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
307 				error = EEXIST;
308 				break;
309 			}
310 			/* Check we don't already trap this VLAN. */
311 			if (priv->vlan_hook[vf->vid] != NULL) {
312 				error = EEXIST;
313 				break;
314 			}
315 			/* Link vlan and hook together. */
316 			NG_HOOK_SET_PRIVATE(hook,
317 			    (void *)(HOOK_VLAN_TAG_SET_MASK |
318 			    EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
319 			priv->vlan_hook[vf->vid] = hook;
320 			break;
321 		case NGM_VLAN_DEL_FILTER:
322 			/* Check that message is long enough. */
323 			if (msg->header.arglen != NG_HOOKSIZ) {
324 				error = EINVAL;
325 				break;
326 			}
327 			/* Check that hook exists and is active. */
328 			hook = ng_findhook(node, (char *)msg->data);
329 			if (hook == NULL) {
330 				error = ENOENT;
331 				break;
332 			}
333 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
334 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
335 				error = ENOENT;
336 				break;
337 			}
338 
339 			KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
340 			    ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
341 			    __func__, (char *)msg->data));
342 
343 			/* Purge a rule that refers to this hook. */
344 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
345 			NG_HOOK_SET_PRIVATE(hook, NULL);
346 			break;
347 		case NGM_VLAN_DEL_VID_FLT:
348 			/* Check that message is long enough. */
349 			if (msg->header.arglen != sizeof(uint16_t)) {
350 				error = EINVAL;
351 				break;
352 			}
353 			vid = (*((uint16_t *)msg->data));
354 			/* Sanity check the VLAN ID value. */
355 			if (vid & ~EVL_VLID_MASK) {
356 				error = EINVAL;
357 				break;
358 			}
359 			/* Check that hook exists and is active. */
360 			hook = priv->vlan_hook[vid];
361 			if (hook == NULL) {
362 				error = ENOENT;
363 				break;
364 			}
365 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
366 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
367 				error = ENOENT;
368 				break;
369 			}
370 
371 			KASSERT(EVL_VLANOFTAG(hook_data) == vid,
372 			    ("%s: NGM_VLAN_DEL_VID_FLT:"
373 			    " Invalid VID Hook = %us, must be: %us\n",
374 			    __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
375 			    vid));
376 
377 			/* Purge a rule that refers to this hook. */
378 			priv->vlan_hook[vid] = NULL;
379 			NG_HOOK_SET_PRIVATE(hook, NULL);
380 			break;
381 		case NGM_VLAN_GET_TABLE:
382 			/* Calculate vlans. */
383 			vlan_count = 0;
384 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
385 				if (priv->vlan_hook[i] != NULL &&
386 				    NG_HOOK_IS_VALID(priv->vlan_hook[i]))
387 					vlan_count ++;
388 			}
389 
390 			/* Allocate memory for response. */
391 			NG_MKRESPONSE(resp, msg, sizeof(*t) +
392 			    vlan_count * sizeof(*t->filter), M_NOWAIT);
393 			if (resp == NULL) {
394 				error = ENOMEM;
395 				break;
396 			}
397 
398 			/* Pack data to response. */
399 			t = (struct ng_vlan_table *)resp->data;
400 			t->n = 0;
401 			vf = &t->filter[0];
402 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
403 				hook = priv->vlan_hook[i];
404 				if (hook == NULL || NG_HOOK_NOT_VALID(hook))
405 					continue;
406 				hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
407 				if (IS_HOOK_VLAN_SET(hook_data) == 0)
408 					continue;
409 
410 				KASSERT(EVL_VLANOFTAG(hook_data) == i,
411 				    ("%s: NGM_VLAN_GET_TABLE:"
412 				    " hook %s VID = %us, must be: %i\n",
413 				    __func__, NG_HOOK_NAME(hook),
414 				    (uint16_t)EVL_VLANOFTAG(hook_data), i));
415 
416 #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
417 				vf->vlan = i;
418 #endif
419 				vf->vid = i;
420 				vf->pcp = EVL_PRIOFTAG(hook_data);
421 				vf->cfi = EVL_CFIOFTAG(hook_data);
422 				strncpy(vf->hook_name,
423 				    NG_HOOK_NAME(hook), NG_HOOKSIZ);
424 				vf ++;
425 				t->n ++;
426 			}
427 			break;
428 		case NGM_VLAN_GET_DECAP:
429 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
430 			if (resp == NULL) {
431 				error = ENOMEM;
432 				break;
433 			}
434 			(*((uint32_t *)resp->data)) = priv->decap_enable;
435 			break;
436 		case NGM_VLAN_SET_DECAP:
437 			if (msg->header.arglen != sizeof(uint32_t)) {
438 				error = EINVAL;
439 				break;
440 			}
441 			priv->decap_enable = (*((uint32_t *)msg->data));
442 			break;
443 		case NGM_VLAN_GET_ENCAP:
444 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
445 			if (resp == NULL) {
446 				error = ENOMEM;
447 				break;
448 			}
449 			(*((uint32_t *)resp->data)) = priv->encap_enable;
450 			break;
451 		case NGM_VLAN_SET_ENCAP:
452 			if (msg->header.arglen != sizeof(uint32_t)) {
453 				error = EINVAL;
454 				break;
455 			}
456 			priv->encap_enable = (*((uint32_t *)msg->data));
457 			break;
458 		case NGM_VLAN_GET_ENCAP_PROTO:
459 			NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
460 			if (resp == NULL) {
461 				error = ENOMEM;
462 				break;
463 			}
464 			(*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
465 			break;
466 		case NGM_VLAN_SET_ENCAP_PROTO:
467 			if (msg->header.arglen != sizeof(uint16_t)) {
468 				error = EINVAL;
469 				break;
470 			}
471 			priv->encap_proto = htons((*((uint16_t *)msg->data)));
472 			break;
473 		default: /* Unknown command. */
474 			error = EINVAL;
475 			break;
476 		}
477 		break;
478 	case NGM_FLOW_COOKIE:
479 	    {
480 		struct ng_mesg *copy;
481 
482 		/*
483 		 * Flow control messages should come only
484 		 * from downstream.
485 		 */
486 
487 		if (lasthook == NULL)
488 			break;
489 		if (lasthook != priv->downstream_hook)
490 			break;
491 		/* Broadcast the event to all uplinks. */
492 		for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
493 			if (priv->vlan_hook[i] == NULL)
494 				continue;
495 
496 			NG_COPYMESSAGE(copy, msg, M_NOWAIT);
497 			if (copy == NULL)
498 				continue;
499 			NG_SEND_MSG_HOOK(error, node, copy,
500 			    priv->vlan_hook[i], 0);
501 		}
502 		break;
503 	    }
504 	default: /* Unknown type cookie. */
505 		error = EINVAL;
506 		break;
507 	}
508 	NG_RESPOND_MSG(error, node, item, resp);
509 	NG_FREE_MSG(msg);
510 	return (error);
511 }
512 
513 static int
514 ng_vlan_rcvdata(hook_p hook, item_p item)
515 {
516 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
517 	struct ether_header *eh;
518 	struct ether_vlan_header *evl;
519 	int error;
520 	uintptr_t hook_data;
521 	uint16_t vid, eth_vtag;
522 	struct mbuf *m;
523 	hook_p dst_hook;
524 
525 	NGI_GET_M(item, m);
526 
527 	/* Make sure we have an entire header. */
528 	error = m_chk(&m, ETHER_HDR_LEN);
529 	if (error != 0)
530 		goto mchk_err;
531 
532 	eh = mtod(m, struct ether_header *);
533 	if (hook == priv->downstream_hook) {
534 		/*
535 		 * If from downstream, select between a match hook
536 		 * or the nomatch hook.
537 		 */
538 
539 		dst_hook = priv->nomatch_hook;
540 
541 		/* Skip packets without tag. */
542 		if ((m->m_flags & M_VLANTAG) == 0 &&
543 		    eh->ether_type != priv->encap_proto) {
544 			if (dst_hook == NULL)
545 				goto net_down;
546 			goto send_packet;
547 		}
548 
549 		/* Process packets with tag. */
550 		if (m->m_flags & M_VLANTAG) {
551 			/*
552 			 * Packet is tagged, m contains a normal
553 			 * Ethernet frame; tag is stored out-of-band.
554 			 */
555 			evl = NULL;
556 			vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
557 		} else { /* eh->ether_type == priv->encap_proto */
558 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
559 			if (error != 0)
560 				goto mchk_err;
561 			evl = mtod(m, struct ether_vlan_header *);
562 			vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
563 		}
564 
565 		if (priv->vlan_hook[vid] != NULL) {
566 			/*
567 			 * VLAN filter: always remove vlan tags and
568 			 * decapsulate packet.
569 			 */
570 			dst_hook = priv->vlan_hook[vid];
571 			if (evl == NULL) { /* m->m_flags & M_VLANTAG */
572 				m->m_pkthdr.ether_vtag = 0;
573 				m->m_flags &= ~M_VLANTAG;
574 				goto send_packet;
575 			}
576 		} else { /* nomatch_hook */
577 			if (dst_hook == NULL)
578 				goto net_down;
579 			if (evl == NULL || priv->decap_enable == 0)
580 				goto send_packet;
581 			/* Save tag out-of-band. */
582 			m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
583 			m->m_flags |= M_VLANTAG;
584 		}
585 
586 		/*
587 		 * Decapsulate:
588 		 * TPID = ether type encap
589 		 * Move DstMAC and SrcMAC to ETHER_TYPE.
590 		 * Before:
591 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
592 		 *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
593 		 * After:
594 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
595 		 *                |-----------| |--------------------|
596 		 */
597 		bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
598 		    (ETHER_ADDR_LEN * 2));
599 		m_adj(m, ETHER_VLAN_ENCAP_LEN);
600 	} else {
601 		/*
602 		 * It is heading towards the downstream.
603 		 * If from nomatch, pass it unmodified.
604 		 * Otherwise, do the VLAN encapsulation.
605 		 */
606 		dst_hook = priv->downstream_hook;
607 		if (dst_hook == NULL)
608 			goto net_down;
609 		if (hook != priv->nomatch_hook) {/* Filter hook. */
610 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
611 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
612 				/*
613 				 * Packet from hook not in filter
614 				 * call addfilter for this hook to fix.
615 				 */
616 				error = EOPNOTSUPP;
617 				goto drop;
618 			}
619 			eth_vtag = (hook_data & VLAN_TAG_MASK);
620 			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
621 				/* Just set packet header tag and send. */
622 				m->m_flags |= M_VLANTAG;
623 				m->m_pkthdr.ether_vtag = eth_vtag;
624 				goto send_packet;
625 			}
626 		} else { /* nomatch_hook */
627 			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
628 			    (m->m_flags & M_VLANTAG) == 0)
629 				goto send_packet;
630 			/* Encapsulate tagged packet. */
631 			eth_vtag = m->m_pkthdr.ether_vtag;
632 			m->m_pkthdr.ether_vtag = 0;
633 			m->m_flags &= ~M_VLANTAG;
634 		}
635 
636 		/*
637 		 * Transform the Ethernet header into an Ethernet header
638 		 * with 802.1Q encapsulation.
639 		 * Mod of: ether_vlanencap.
640 		 *
641 		 * TPID = ether type encap
642 		 * Move DstMAC and SrcMAC from ETHER_TYPE.
643 		 * Before:
644 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
645 		 *  <<<<<<<<<<<<< |-----------| |--------------------|
646 		 * After:
647 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
648 		 *  |-----------| |-- inserted tag --| |--------------------|
649 		 */
650 		M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_NOWAIT);
651 		if (m == NULL)
652 			error = ENOMEM;
653 		else
654 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
655 		if (error != 0)
656 			goto mchk_err;
657 
658 		evl = mtod(m, struct ether_vlan_header *);
659 		bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
660 		    (char *)evl, (ETHER_ADDR_LEN * 2));
661 		evl->evl_encap_proto = priv->encap_proto;
662 		evl->evl_tag = htons(eth_vtag);
663 	}
664 
665 send_packet:
666 	NG_FWD_NEW_DATA(error, item, dst_hook, m);
667 	return (error);
668 net_down:
669 	error = ENETDOWN;
670 drop:
671 	m_freem(m);
672 mchk_err:
673 	NG_FREE_ITEM(item);
674 	return (error);
675 }
676 
677 static int
678 ng_vlan_shutdown(node_p node)
679 {
680 	const priv_p priv = NG_NODE_PRIVATE(node);
681 
682 	NG_NODE_SET_PRIVATE(node, NULL);
683 	NG_NODE_UNREF(node);
684 	free(priv, M_NETGRAPH);
685 	return (0);
686 }
687 
688 static int
689 ng_vlan_disconnect(hook_p hook)
690 {
691 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
692 	uintptr_t hook_data;
693 
694 	if (hook == priv->downstream_hook)
695 		priv->downstream_hook = NULL;
696 	else if (hook == priv->nomatch_hook)
697 		priv->nomatch_hook = NULL;
698 	else {
699 		/* Purge a rule that refers to this hook. */
700 		hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
701 		if (IS_HOOK_VLAN_SET(hook_data))
702 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
703 	}
704 	NG_HOOK_SET_PRIVATE(hook, NULL);
705 	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
706 	    (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
707 		ng_rmnode_self(NG_HOOK_NODE(hook));
708 	return (0);
709 }
710