xref: /freebsd/sys/netgraph/ng_vlan.c (revision 4f52dfbb8d6c4d446500c5b097e3806ec219fbd4)
1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
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 /*
199  * Helper functions.
200  */
201 
202 static __inline int
203 m_chk(struct mbuf **mp, int len)
204 {
205 
206 	if ((*mp)->m_pkthdr.len < len) {
207 		m_freem((*mp));
208 		(*mp) = NULL;
209 		return (EINVAL);
210 	}
211 	if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
212 		return (ENOBUFS);
213 
214 	return (0);
215 }
216 
217 
218 /*
219  * Netgraph node functions.
220  */
221 
222 static int
223 ng_vlan_constructor(node_p node)
224 {
225 	priv_p priv;
226 
227 	priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
228 	priv->decap_enable = 0;
229 	priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
230 	priv->encap_proto = htons(ETHERTYPE_VLAN);
231 	NG_NODE_SET_PRIVATE(node, priv);
232 	return (0);
233 }
234 
235 static int
236 ng_vlan_newhook(node_p node, hook_p hook, const char *name)
237 {
238 	const priv_p priv = NG_NODE_PRIVATE(node);
239 
240 	if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
241 		priv->downstream_hook = hook;
242 	else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
243 		priv->nomatch_hook = hook;
244 	else {
245 		/*
246 		 * Any other hook name is valid and can
247 		 * later be associated with a filter rule.
248 		 */
249 	}
250 	NG_HOOK_SET_PRIVATE(hook, NULL);
251 	return (0);
252 }
253 
254 static int
255 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
256 {
257 	const priv_p priv = NG_NODE_PRIVATE(node);
258 	struct ng_mesg *msg, *resp = NULL;
259 	struct ng_vlan_filter *vf;
260 	hook_p hook;
261 	struct ng_vlan_table *t;
262 	uintptr_t hook_data;
263 	int i, vlan_count;
264 	uint16_t vid;
265 	int error = 0;
266 
267 	NGI_GET_MSG(item, msg);
268 	/* Deal with message according to cookie and command. */
269 	switch (msg->header.typecookie) {
270 	case NGM_VLAN_COOKIE:
271 		switch (msg->header.cmd) {
272 		case NGM_VLAN_ADD_FILTER:
273 			/* Check that message is long enough. */
274 			if (msg->header.arglen != sizeof(*vf)) {
275 				error = EINVAL;
276 				break;
277 			}
278 			vf = (struct ng_vlan_filter *)msg->data;
279 			/* Sanity check the VLAN ID value. */
280 #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
281 			if (vf->vid == 0 && vf->vid != vf->vlan) {
282 				vf->vid = vf->vlan;
283 			} else if (vf->vid != 0 && vf->vlan != 0 &&
284 			    vf->vid != vf->vlan) {
285 				error = EINVAL;
286 				break;
287 			}
288 #endif
289 			if (vf->vid & ~EVL_VLID_MASK ||
290 			    vf->pcp & ~7 ||
291 			    vf->cfi & ~1) {
292 				error = EINVAL;
293 				break;
294 			}
295 			/* Check that a referenced hook exists. */
296 			hook = ng_findhook(node, vf->hook_name);
297 			if (hook == NULL) {
298 				error = ENOENT;
299 				break;
300 			}
301 			/* And is not one of the special hooks. */
302 			if (hook == priv->downstream_hook ||
303 			    hook == priv->nomatch_hook) {
304 				error = EINVAL;
305 				break;
306 			}
307 			/* And is not already in service. */
308 			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
309 				error = EEXIST;
310 				break;
311 			}
312 			/* Check we don't already trap this VLAN. */
313 			if (priv->vlan_hook[vf->vid] != NULL) {
314 				error = EEXIST;
315 				break;
316 			}
317 			/* Link vlan and hook together. */
318 			NG_HOOK_SET_PRIVATE(hook,
319 			    (void *)(HOOK_VLAN_TAG_SET_MASK |
320 			    EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
321 			priv->vlan_hook[vf->vid] = hook;
322 			break;
323 		case NGM_VLAN_DEL_FILTER:
324 			/* Check that message is long enough. */
325 			if (msg->header.arglen != NG_HOOKSIZ) {
326 				error = EINVAL;
327 				break;
328 			}
329 			/* Check that hook exists and is active. */
330 			hook = ng_findhook(node, (char *)msg->data);
331 			if (hook == NULL) {
332 				error = ENOENT;
333 				break;
334 			}
335 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
336 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
337 				error = ENOENT;
338 				break;
339 			}
340 
341 			KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
342 			    ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
343 			    __func__, (char *)msg->data));
344 
345 			/* Purge a rule that refers to this hook. */
346 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
347 			NG_HOOK_SET_PRIVATE(hook, NULL);
348 			break;
349 		case NGM_VLAN_DEL_VID_FLT:
350 			/* Check that message is long enough. */
351 			if (msg->header.arglen != sizeof(uint16_t)) {
352 				error = EINVAL;
353 				break;
354 			}
355 			vid = (*((uint16_t *)msg->data));
356 			/* Sanity check the VLAN ID value. */
357 			if (vid & ~EVL_VLID_MASK) {
358 				error = EINVAL;
359 				break;
360 			}
361 			/* Check that hook exists and is active. */
362 			hook = priv->vlan_hook[vid];
363 			if (hook == NULL) {
364 				error = ENOENT;
365 				break;
366 			}
367 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
368 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
369 				error = ENOENT;
370 				break;
371 			}
372 
373 			KASSERT(EVL_VLANOFTAG(hook_data) == vid,
374 			    ("%s: NGM_VLAN_DEL_VID_FLT:"
375 			    " Invalid VID Hook = %us, must be: %us\n",
376 			    __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
377 			    vid));
378 
379 			/* Purge a rule that refers to this hook. */
380 			priv->vlan_hook[vid] = NULL;
381 			NG_HOOK_SET_PRIVATE(hook, NULL);
382 			break;
383 		case NGM_VLAN_GET_TABLE:
384 			/* Calculate vlans. */
385 			vlan_count = 0;
386 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
387 				if (priv->vlan_hook[i] != NULL &&
388 				    NG_HOOK_IS_VALID(priv->vlan_hook[i]))
389 					vlan_count ++;
390 			}
391 
392 			/* Allocate memory for response. */
393 			NG_MKRESPONSE(resp, msg, sizeof(*t) +
394 			    vlan_count * sizeof(*t->filter), M_NOWAIT);
395 			if (resp == NULL) {
396 				error = ENOMEM;
397 				break;
398 			}
399 
400 			/* Pack data to response. */
401 			t = (struct ng_vlan_table *)resp->data;
402 			t->n = 0;
403 			vf = &t->filter[0];
404 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
405 				hook = priv->vlan_hook[i];
406 				if (hook == NULL || NG_HOOK_NOT_VALID(hook))
407 					continue;
408 				hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
409 				if (IS_HOOK_VLAN_SET(hook_data) == 0)
410 					continue;
411 
412 				KASSERT(EVL_VLANOFTAG(hook_data) == i,
413 				    ("%s: NGM_VLAN_GET_TABLE:"
414 				    " hook %s VID = %us, must be: %i\n",
415 				    __func__, NG_HOOK_NAME(hook),
416 				    (uint16_t)EVL_VLANOFTAG(hook_data), i));
417 
418 #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
419 				vf->vlan = i;
420 #endif
421 				vf->vid = i;
422 				vf->pcp = EVL_PRIOFTAG(hook_data);
423 				vf->cfi = EVL_CFIOFTAG(hook_data);
424 				strncpy(vf->hook_name,
425 				    NG_HOOK_NAME(hook), NG_HOOKSIZ);
426 				vf ++;
427 				t->n ++;
428 			}
429 			break;
430 		case NGM_VLAN_GET_DECAP:
431 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
432 			if (resp == NULL) {
433 				error = ENOMEM;
434 				break;
435 			}
436 			(*((uint32_t *)resp->data)) = priv->decap_enable;
437 			break;
438 		case NGM_VLAN_SET_DECAP:
439 			if (msg->header.arglen != sizeof(uint32_t)) {
440 				error = EINVAL;
441 				break;
442 			}
443 			priv->decap_enable = (*((uint32_t *)msg->data));
444 			break;
445 		case NGM_VLAN_GET_ENCAP:
446 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
447 			if (resp == NULL) {
448 				error = ENOMEM;
449 				break;
450 			}
451 			(*((uint32_t *)resp->data)) = priv->encap_enable;
452 			break;
453 		case NGM_VLAN_SET_ENCAP:
454 			if (msg->header.arglen != sizeof(uint32_t)) {
455 				error = EINVAL;
456 				break;
457 			}
458 			priv->encap_enable = (*((uint32_t *)msg->data));
459 			break;
460 		case NGM_VLAN_GET_ENCAP_PROTO:
461 			NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
462 			if (resp == NULL) {
463 				error = ENOMEM;
464 				break;
465 			}
466 			(*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
467 			break;
468 		case NGM_VLAN_SET_ENCAP_PROTO:
469 			if (msg->header.arglen != sizeof(uint16_t)) {
470 				error = EINVAL;
471 				break;
472 			}
473 			priv->encap_proto = htons((*((uint16_t *)msg->data)));
474 			break;
475 		default: /* Unknown command. */
476 			error = EINVAL;
477 			break;
478 		}
479 		break;
480 	case NGM_FLOW_COOKIE:
481 	    {
482 		struct ng_mesg *copy;
483 
484 		/*
485 		 * Flow control messages should come only
486 		 * from downstream.
487 		 */
488 
489 		if (lasthook == NULL)
490 			break;
491 		if (lasthook != priv->downstream_hook)
492 			break;
493 		/* Broadcast the event to all uplinks. */
494 		for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
495 			if (priv->vlan_hook[i] == NULL)
496 				continue;
497 
498 			NG_COPYMESSAGE(copy, msg, M_NOWAIT);
499 			if (copy == NULL)
500 				continue;
501 			NG_SEND_MSG_HOOK(error, node, copy,
502 			    priv->vlan_hook[i], 0);
503 		}
504 		break;
505 	    }
506 	default: /* Unknown type cookie. */
507 		error = EINVAL;
508 		break;
509 	}
510 	NG_RESPOND_MSG(error, node, item, resp);
511 	NG_FREE_MSG(msg);
512 	return (error);
513 }
514 
515 static int
516 ng_vlan_rcvdata(hook_p hook, item_p item)
517 {
518 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
519 	struct ether_header *eh;
520 	struct ether_vlan_header *evl;
521 	int error;
522 	uintptr_t hook_data;
523 	uint16_t vid, eth_vtag;
524 	struct mbuf *m;
525 	hook_p dst_hook;
526 
527 
528 	NGI_GET_M(item, m);
529 
530 	/* Make sure we have an entire header. */
531 	error = m_chk(&m, ETHER_HDR_LEN);
532 	if (error != 0)
533 		goto mchk_err;
534 
535 	eh = mtod(m, struct ether_header *);
536 	if (hook == priv->downstream_hook) {
537 		/*
538 		 * If from downstream, select between a match hook
539 		 * or the nomatch hook.
540 		 */
541 
542 		dst_hook = priv->nomatch_hook;
543 
544 		/* Skip packets without tag. */
545 		if ((m->m_flags & M_VLANTAG) == 0 &&
546 		    eh->ether_type != priv->encap_proto) {
547 			if (dst_hook == NULL)
548 				goto net_down;
549 			goto send_packet;
550 		}
551 
552 		/* Process packets with tag. */
553 		if (m->m_flags & M_VLANTAG) {
554 			/*
555 			 * Packet is tagged, m contains a normal
556 			 * Ethernet frame; tag is stored out-of-band.
557 			 */
558 			evl = NULL;
559 			vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
560 		} else { /* eh->ether_type == priv->encap_proto */
561 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
562 			if (error != 0)
563 				goto mchk_err;
564 			evl = mtod(m, struct ether_vlan_header *);
565 			vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
566 		}
567 
568 		if (priv->vlan_hook[vid] != NULL) {
569 			/*
570 			 * VLAN filter: always remove vlan tags and
571 			 * decapsulate packet.
572 			 */
573 			dst_hook = priv->vlan_hook[vid];
574 			if (evl == NULL) { /* m->m_flags & M_VLANTAG */
575 				m->m_pkthdr.ether_vtag = 0;
576 				m->m_flags &= ~M_VLANTAG;
577 				goto send_packet;
578 			}
579 		} else { /* nomatch_hook */
580 			if (dst_hook == NULL)
581 				goto net_down;
582 			if (evl == NULL || priv->decap_enable == 0)
583 				goto send_packet;
584 			/* Save tag out-of-band. */
585 			m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
586 			m->m_flags |= M_VLANTAG;
587 		}
588 
589 		/*
590 		 * Decapsulate:
591 		 * TPID = ether type encap
592 		 * Move DstMAC and SrcMAC to ETHER_TYPE.
593 		 * Before:
594 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
595 		 *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
596 		 * After:
597 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
598 		 *                |-----------| |--------------------|
599 		 */
600 		bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
601 		    (ETHER_ADDR_LEN * 2));
602 		m_adj(m, ETHER_VLAN_ENCAP_LEN);
603 	} else {
604 		/*
605 		 * It is heading towards the downstream.
606 		 * If from nomatch, pass it unmodified.
607 		 * Otherwise, do the VLAN encapsulation.
608 		 */
609 		dst_hook = priv->downstream_hook;
610 		if (dst_hook == NULL)
611 			goto net_down;
612 		if (hook != priv->nomatch_hook) {/* Filter hook. */
613 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
614 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
615 				/*
616 				 * Packet from hook not in filter
617 				 * call addfilter for this hook to fix.
618 				 */
619 				error = EOPNOTSUPP;
620 				goto drop;
621 			}
622 			eth_vtag = (hook_data & VLAN_TAG_MASK);
623 			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
624 				/* Just set packet header tag and send. */
625 				m->m_flags |= M_VLANTAG;
626 				m->m_pkthdr.ether_vtag = eth_vtag;
627 				goto send_packet;
628 			}
629 		} else { /* nomatch_hook */
630 			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
631 			    (m->m_flags & M_VLANTAG) == 0)
632 				goto send_packet;
633 			/* Encapsulate tagged packet. */
634 			eth_vtag = m->m_pkthdr.ether_vtag;
635 			m->m_pkthdr.ether_vtag = 0;
636 			m->m_flags &= ~M_VLANTAG;
637 		}
638 
639 		/*
640 		 * Transform the Ethernet header into an Ethernet header
641 		 * with 802.1Q encapsulation.
642 		 * Mod of: ether_vlanencap.
643 		 *
644 		 * TPID = ether type encap
645 		 * Move DstMAC and SrcMAC from ETHER_TYPE.
646 		 * Before:
647 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
648 		 *  <<<<<<<<<<<<< |-----------| |--------------------|
649 		 * After:
650 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
651 		 *  |-----------| |-- inserted tag --| |--------------------|
652 		 */
653 		M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_NOWAIT);
654 		if (m == NULL)
655 			error = ENOMEM;
656 		else
657 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
658 		if (error != 0)
659 			goto mchk_err;
660 
661 		evl = mtod(m, struct ether_vlan_header *);
662 		bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
663 		    (char *)evl, (ETHER_ADDR_LEN * 2));
664 		evl->evl_encap_proto = priv->encap_proto;
665 		evl->evl_tag = htons(eth_vtag);
666 	}
667 
668 send_packet:
669 	NG_FWD_NEW_DATA(error, item, dst_hook, m);
670 	return (error);
671 net_down:
672 	error = ENETDOWN;
673 drop:
674 	m_freem(m);
675 mchk_err:
676 	NG_FREE_ITEM(item);
677 	return (error);
678 }
679 
680 static int
681 ng_vlan_shutdown(node_p node)
682 {
683 	const priv_p priv = NG_NODE_PRIVATE(node);
684 
685 	NG_NODE_SET_PRIVATE(node, NULL);
686 	NG_NODE_UNREF(node);
687 	free(priv, M_NETGRAPH);
688 	return (0);
689 }
690 
691 static int
692 ng_vlan_disconnect(hook_p hook)
693 {
694 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
695 	uintptr_t hook_data;
696 
697 	if (hook == priv->downstream_hook)
698 		priv->downstream_hook = NULL;
699 	else if (hook == priv->nomatch_hook)
700 		priv->nomatch_hook = NULL;
701 	else {
702 		/* Purge a rule that refers to this hook. */
703 		hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
704 		if (IS_HOOK_VLAN_SET(hook_data))
705 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
706 	}
707 	NG_HOOK_SET_PRIVATE(hook, NULL);
708 	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
709 	    (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
710 		ng_rmnode_self(NG_HOOK_NODE(hook));
711 	return (0);
712 }
713