Upgrade the netgraph vlan node to support 802.1q, encapsulation type,

PCP and CFI fields.

* Ethernet_type for VLAN encapsulation is tunable, default is 0x8100;
* PCP (Priority code point) and CFI (canonical format indicator) is
  tunable per VID;
* Tunable encapsulation to support 802.1q
* Encapsulation/Decapsulation code improvements

New messages have been added for this netgraph node to support the
new features.

However, the legacy "vlan" id is still supported and compiled in by
default.  It can be disabled in a future release.

TODO:

* Documentation
* Examples

PR:		kern/161908
Submitted by:	Ivan <rozhuk.im@gmail.com>

1 files changed, 388 insertions, 144 deletions

diff --git a/sys/netgraph/ng_vlan.c b/sys/netgraph/ng_vlan.c
index f9de94f..b07a603 100644
--- a/sys/netgraph/ng_vlan.c
+++ b/sys/netgraph/ng_vlan.c
@@ -1,5 +1,6 @@
 /*-
  * Copyright (c) 2003 IPNET Internet Communication Company
+ * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -46,6 +47,22 @@
 #include <netgraph/ng_vlan.h>
 #include <netgraph/netgraph.h>
 
+struct ng_vlan_private {
+	hook_p		downstream_hook;
+	hook_p		nomatch_hook;
+	uint32_t	decap_enable;
+	uint32_t	encap_enable;
+	uint16_t	encap_proto;
+	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];
+};
+typedef struct ng_vlan_private *priv_p;
+
+#define	ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)
+#define	VLAN_TAG_MASK	0xFFFF
+#define	HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
+#define	IS_HOOK_VLAN_SET(hdata) \
+	    ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) == HOOK_VLAN_TAG_SET_MASK)
+
 static ng_constructor_t	ng_vlan_constructor;
 static ng_rcvmsg_t	ng_vlan_rcvmsg;
 static ng_shutdown_t	ng_vlan_shutdown;
@@ -110,6 +127,55 @@ static const struct ng_cmdlist ng_vlan_cmdlist[] = {
 	  NULL,
 	  &ng_vlan_table_type
 	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_DEL_VID_FLT,
+	  "delvidflt",
+	  &ng_parse_uint16_type,
+	  NULL
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_GET_DECAP,
+	  "getdecap",
+	  NULL,
+	  &ng_parse_hint32_type
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_SET_DECAP,
+	  "setdecap",
+	  &ng_parse_hint32_type,
+	  NULL
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_GET_ENCAP,
+	  "getencap",
+	  NULL,
+	  &ng_parse_hint32_type
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_SET_ENCAP,
+	  "setencap",
+	  &ng_parse_hint32_type,
+	  NULL
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_GET_ENCAP_PROTO,
+	  "getencapproto",
+	  NULL,
+	  &ng_parse_hint16_type
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_SET_ENCAP_PROTO,
+	  "setencapproto",
+	  &ng_parse_hint16_type,
+	  NULL
+	},
 	{ 0 }
 };
 
@@ -126,44 +192,40 @@ static struct ng_type ng_vlan_typestruct = {
 };
 NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
 
-struct filter {
-	LIST_ENTRY(filter) next;
-	u_int16_t	vlan;
-	hook_p		hook;
-};
-
-#define	HASHSIZE	16
-#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)
-LIST_HEAD(filterhead, filter);
 
-typedef struct {
-	hook_p		downstream_hook;
-	hook_p		nomatch_hook;
-	struct filterhead hashtable[HASHSIZE];
-	u_int32_t	nent;
-} *priv_p;
+/*
+ * Helper functions.
+ */
 
-static struct filter *
-ng_vlan_findentry(priv_p priv, u_int16_t vlan)
+static __inline int
+m_chk(struct mbuf **mp, int len)
 {
-	struct filterhead *chain = &priv->hashtable[HASH(vlan)];
-	struct filter *f;
 
-	LIST_FOREACH(f, chain, next)
-		if (f->vlan == vlan)
-			return (f);
-	return (NULL);
+	if ((*mp)->m_pkthdr.len < len) {
+		m_freem((*mp));
+		(*mp) = NULL;
+		return (EINVAL);
+	}
+	if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
+		return (ENOBUFS);
+
+	return (0);
 }
 
+
+/*
+ * Netgraph node functions.
+ */
+
 static int
 ng_vlan_constructor(node_p node)
 {
 	priv_p priv;
-	int i;
 
 	priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
-	for (i = 0; i < HASHSIZE; i++)
-		LIST_INIT(&priv->hashtable[i]);
+	priv->decap_enable = 0;
+	priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
+	priv->encap_proto = htons(ETHERTYPE_VLAN);
 	NG_NODE_SET_PRIVATE(node, priv);
 	return (0);
 }
@@ -191,13 +253,14 @@ static int
 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 {
 	const priv_p priv = NG_NODE_PRIVATE(node);
-	int error = 0;
 	struct ng_mesg *msg, *resp = NULL;
 	struct ng_vlan_filter *vf;
-	struct filter *f;
 	hook_p hook;
 	struct ng_vlan_table *t;
-	int i;
+	uintptr_t hook_data;
+	int i, vlan_count;
+	uint16_t vid;
+	int error = 0;
 
 	NGI_GET_MSG(item, msg);
 	/* Deal with message according to cookie and command. */
@@ -212,12 +275,23 @@ ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			}
 			vf = (struct ng_vlan_filter *)msg->data;
 			/* Sanity check the VLAN ID value. */
-			if (vf->vlan & ~EVL_VLID_MASK) {
+#ifdef	NG_VLAN_USE_OLD_VLAN_NAME
+			if (vf->vid == 0 && vf->vid != vf->vlan) {
+				vf->vid = vf->vlan;
+			} else if (vf->vid != 0 && vf->vlan != 0 &&
+			    vf->vid != vf->vlan) {
+				error = EINVAL;
+				break;
+			}
+#endif
+			if (vf->vid & ~EVL_VLID_MASK ||
+			    vf->pcp & ~7 ||
+			    vf->cfi & ~1) {
 				error = EINVAL;
 				break;
 			}
 			/* Check that a referenced hook exists. */
-			hook = ng_findhook(node, vf->hook);
+			hook = ng_findhook(node, vf->hook_name);
 			if (hook == NULL) {
 				error = ENOENT;
 				break;
@@ -229,30 +303,20 @@ ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				break;
 			}
 			/* And is not already in service. */
-			if (NG_HOOK_PRIVATE(hook) != NULL) {
+			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
 				error = EEXIST;
 				break;
 			}
 			/* Check we don't already trap this VLAN. */
-			if (ng_vlan_findentry(priv, vf->vlan)) {
+			if (priv->vlan_hook[vf->vid] != NULL) {
 				error = EEXIST;
 				break;
 			}
-			/* Create filter. */
-			f = malloc(sizeof(*f),
-			    M_NETGRAPH, M_NOWAIT | M_ZERO);
-			if (f == NULL) {
-				error = ENOMEM;
-				break;
-			}
-			/* Link filter and hook together. */
-			f->hook = hook;
-			f->vlan = vf->vlan;
-			NG_HOOK_SET_PRIVATE(hook, f);
-			/* Register filter in a hash table. */
-			LIST_INSERT_HEAD(
-			    &priv->hashtable[HASH(f->vlan)], f, next);
-			priv->nent++;
+			/* Link vlan and hook together. */
+			NG_HOOK_SET_PRIVATE(hook,
+			    (void *)(HOOK_VLAN_TAG_SET_MASK |
+			    EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
+			priv->vlan_hook[vf->vid] = hook;
 			break;
 		case NGM_VLAN_DEL_FILTER:
 			/* Check that message is long enough. */
@@ -262,37 +326,151 @@ ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			}
 			/* Check that hook exists and is active. */
 			hook = ng_findhook(node, (char *)msg->data);
-			if (hook == NULL ||
-			    (f = NG_HOOK_PRIVATE(hook)) == NULL) {
+			if (hook == NULL) {
 				error = ENOENT;
 				break;
 			}
+			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
+				error = ENOENT;
+				break;
+			}
+
+			KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
+			    ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
+			    __func__, (char *)msg->data));
+
 			/* Purge a rule that refers to this hook. */
+			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
+			NG_HOOK_SET_PRIVATE(hook, NULL);
+			break;
+		case NGM_VLAN_DEL_VID_FLT:
+			/* Check that message is long enough. */
+			if (msg->header.arglen != sizeof(uint16_t)) {
+				error = EINVAL;
+				break;
+			}
+			vid = (*((uint16_t *)msg->data));
+			/* Sanity check the VLAN ID value. */
+			if (vid & ~EVL_VLID_MASK) {
+				error = EINVAL;
+				break;
+			}
+			/* Check that hook exists and is active. */
+			hook = priv->vlan_hook[vid];
+			if (hook == NULL) {
+				error = ENOENT;
+				break;
+			}
+			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
+				error = ENOENT;
+				break;
+			}
+
+			KASSERT(EVL_VLANOFTAG(hook_data) == vid,
+			    ("%s: NGM_VLAN_DEL_VID_FLT:"
+			    " Invalid VID Hook = %us, must be: %us\n",
+			    __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
+			    vid));
+
+			/* Purge a rule that refers to this hook. */
+			priv->vlan_hook[vid] = NULL;
 			NG_HOOK_SET_PRIVATE(hook, NULL);
-			LIST_REMOVE(f, next);
-			priv->nent--;
-			free(f, M_NETGRAPH);
 			break;
 		case NGM_VLAN_GET_TABLE:
+			/* Calculate vlans. */
+			vlan_count = 0;
+			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
+				if (priv->vlan_hook[i] != NULL &&
+				    NG_HOOK_IS_VALID(priv->vlan_hook[i]))
+					vlan_count ++;
+			}
+
+			/* Allocate memory for responce. */
 			NG_MKRESPONSE(resp, msg, sizeof(*t) +
-			    priv->nent * sizeof(*t->filter), M_NOWAIT);
+			    vlan_count * sizeof(*t->filter), M_NOWAIT);
 			if (resp == NULL) {
 				error = ENOMEM;
 				break;
 			}
+
+			/* Pack data to responce. */
 			t = (struct ng_vlan_table *)resp->data;
-			t->n = priv->nent;
+			t->n = 0;
 			vf = &t->filter[0];
-			for (i = 0; i < HASHSIZE; i++) {
-				LIST_FOREACH(f, &priv->hashtable[i], next) {
-					vf->vlan = f->vlan;
-					strncpy(vf->hook, NG_HOOK_NAME(f->hook),
-					    NG_HOOKSIZ);
-					vf++;
-				}
+			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
+				hook = priv->vlan_hook[i];
+				if (hook == NULL || NG_HOOK_NOT_VALID(hook))
+					continue;
+				hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+				if (IS_HOOK_VLAN_SET(hook_data) == 0)
+					continue;
+
+				KASSERT(EVL_VLANOFTAG(hook_data) == i,
+				    ("%s: NGM_VLAN_GET_TABLE:"
+				    " hook %s VID = %us, must be: %i\n",
+				    __func__, NG_HOOK_NAME(hook),
+				    (uint16_t)EVL_VLANOFTAG(hook_data), i));
+
+#ifdef	NG_VLAN_USE_OLD_VLAN_NAME
+				vf->vlan = i;
+#endif
+				vf->vid = i;
+				vf->pcp = EVL_PRIOFTAG(hook_data);
+				vf->cfi = EVL_CFIOFTAG(hook_data);
+				strncpy(vf->hook_name,
+				    NG_HOOK_NAME(hook), NG_HOOKSIZ);
+				vf ++;
+				t->n ++;
+			}
+			break;
+		case NGM_VLAN_GET_DECAP:
+			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+			(*((uint32_t *)resp->data)) = priv->decap_enable;
+			break;
+		case NGM_VLAN_SET_DECAP:
+			if (msg->header.arglen != sizeof(uint32_t)) {
+				error = EINVAL;
+				break;
 			}
+			priv->decap_enable = (*((uint32_t *)msg->data));
 			break;
-		default:		/* Unknown command. */
+		case NGM_VLAN_GET_ENCAP:
+			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+			(*((uint32_t *)resp->data)) = priv->encap_enable;
+			break;
+		case NGM_VLAN_SET_ENCAP:
+			if (msg->header.arglen != sizeof(uint32_t)) {
+				error = EINVAL;
+				break;
+			}
+			priv->encap_enable = (*((uint32_t *)msg->data));
+			break;
+		case NGM_VLAN_GET_ENCAP_PROTO:
+			NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+			(*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
+			break;
+		case NGM_VLAN_SET_ENCAP_PROTO:
+			if (msg->header.arglen != sizeof(uint16_t)) {
+				error = EINVAL;
+				break;
+			}
+			priv->encap_proto = htons((*((uint16_t *)msg->data)));
+			break;
+		default: /* Unknown command. */
 			error = EINVAL;
 			break;
 		}
@@ -300,8 +478,6 @@ ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 	case NGM_FLOW_COOKIE:
 	    {
 		struct ng_mesg *copy;
-		struct filterhead *chain;
-		struct filter *f;
 
 		/*
 		 * Flow control messages should come only
@@ -312,20 +488,20 @@ ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			break;
 		if (lasthook != priv->downstream_hook)
 			break;
-
 		/* Broadcast the event to all uplinks. */
-		for (i = 0, chain = priv->hashtable; i < HASHSIZE;
-		    i++, chain++)
-		LIST_FOREACH(f, chain, next) {
+		for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
+			if (priv->vlan_hook[i] == NULL)
+				continue;
+
 			NG_COPYMESSAGE(copy, msg, M_NOWAIT);
 			if (copy == NULL)
 				continue;
-			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);
+			NG_SEND_MSG_HOOK(error, node, copy,
+			    priv->vlan_hook[i], 0);
 		}
-
 		break;
 	    }
-	default:			/* Unknown type cookie. */
+	default: /* Unknown type cookie. */
 		error = EINVAL;
 		break;
 	}
@@ -339,93 +515,163 @@ ng_vlan_rcvdata(hook_p hook, item_p item)
 {
 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 	struct ether_header *eh;
-	struct ether_vlan_header *evl = NULL;
+	struct ether_vlan_header *evl;
 	int error;
-	u_int16_t vlan;
+	uintptr_t hook_data;
+	uint16_t vid, eth_vtag;
 	struct mbuf *m;
-	struct filter *f;
+	hook_p dst_hook;
+
 
-	/* Make sure we have an entire header. */
 	NGI_GET_M(item, m);
-	if (m->m_len < sizeof(*eh) &&
-	    (m = m_pullup(m, sizeof(*eh))) == NULL) {
-		NG_FREE_ITEM(item);
-		return (EINVAL);
-	}
+
+	/* Make sure we have an entire header. */
+	error = m_chk(&m, ETHER_HDR_LEN);
+	if (error != 0)
+		goto mchk_err;
+
 	eh = mtod(m, struct ether_header *);
 	if (hook == priv->downstream_hook) {
 		/*
 		 * If from downstream, select between a match hook
 		 * or the nomatch hook.
 		 */
-		if (m->m_flags & M_VLANTAG ||
-		    eh->ether_type == htons(ETHERTYPE_VLAN)) {
-			if (m->m_flags & M_VLANTAG) {
-				/*
-				 * Packet is tagged, m contains a normal
-				 * Ethernet frame; tag is stored out-of-band.
-				 */
-				vlan = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
-			} else {
-				if (m->m_len < sizeof(*evl) &&
-				    (m = m_pullup(m, sizeof(*evl))) == NULL) {
-					NG_FREE_ITEM(item);
-					return (EINVAL);
-				}
-				evl = mtod(m, struct ether_vlan_header *);
-				vlan = EVL_VLANOFTAG(ntohs(evl->evl_tag));
-			}
-			if ((f = ng_vlan_findentry(priv, vlan)) != NULL) {
-				if (m->m_flags & M_VLANTAG) {
-					m->m_pkthdr.ether_vtag = 0;
-					m->m_flags &= ~M_VLANTAG;
-				} else {
-					evl->evl_encap_proto = evl->evl_proto;
-					bcopy(mtod(m, caddr_t),
-					    mtod(m, caddr_t) +
-					    ETHER_VLAN_ENCAP_LEN,
-					    ETHER_HDR_LEN);
-					m_adj(m, ETHER_VLAN_ENCAP_LEN);
-				}
+
+		dst_hook = priv->nomatch_hook;
+
+		/* Skip packets without tag. */
+		if ((m->m_flags & M_VLANTAG) == 0 &&
+		    eh->ether_type != priv->encap_proto) {
+			if (dst_hook == NULL)
+				goto net_down;
+			goto send_packet;
+		}
+
+		/* Process packets with tag. */
+		if (m->m_flags & M_VLANTAG) {
+			/*
+			 * Packet is tagged, m contains a normal
+			 * Ethernet frame; tag is stored out-of-band.
+			 */
+			evl = NULL;
+			vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
+		} else { /* eh->ether_type == priv->encap_proto */
+			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
+			if (error != 0)
+				goto mchk_err;
+			evl = mtod(m, struct ether_vlan_header *);
+			vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
+		}
+
+		if (priv->vlan_hook[vid] != NULL) {
+			/*
+			 * VLAN filter: allways remove vlan tags and
+			 * decapsulate packet.
+			 */
+			dst_hook = priv->vlan_hook[vid];
+			if (evl == NULL) { /* m->m_flags & M_VLANTAG */
+				m->m_pkthdr.ether_vtag = 0;
+				m->m_flags &= ~M_VLANTAG;
+				goto send_packet;
 			}
-		} else
-			f = NULL;
-		if (f != NULL)
-			NG_FWD_NEW_DATA(error, item, f->hook, m);
-		else
-			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);
+		} else { /* nomatch_hook */
+			if (dst_hook == NULL)
+				goto net_down;
+			if (evl == NULL || priv->decap_enable == 0)
+				goto send_packet;
+			/* Save tag out-of-band. */
+			m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
+			m->m_flags |= M_VLANTAG;
+		}
+
+		/*
+		 * Decapsulate:
+		 * TPID = ether type encap
+		 * Move DstMAC and SrcMAC to ETHER_TYPE.
+		 * Before:
+		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
+		 *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
+		 * After:
+		 *  [free space ] [dmac] [smac] [ether_type] [payload]
+		 *                |-----------| |--------------------|
+		 */
+		bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
+		    (ETHER_ADDR_LEN * 2));
+		m_adj(m, ETHER_VLAN_ENCAP_LEN);
 	} else {
 		/*
 		 * It is heading towards the downstream.
 		 * If from nomatch, pass it unmodified.
 		 * Otherwise, do the VLAN encapsulation.
 		 */
-		if (hook != priv->nomatch_hook) {
-			if ((f = NG_HOOK_PRIVATE(hook)) == NULL) {
-				NG_FREE_ITEM(item);
-				NG_FREE_M(m);
-				return (EOPNOTSUPP);
+		dst_hook = priv->downstream_hook;
+		if (dst_hook == NULL)
+			goto net_down;
+		if (hook != priv->nomatch_hook) {/* Filter hook. */
+			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
+				/*
+				 * Packet from hook not in filter
+				 * call addfilter for this hook to fix.
+				 */
+				error = EOPNOTSUPP;
+				goto drop;
 			}
-			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
-			/* M_PREPEND takes care of m_len and m_pkthdr.len. */
-			if (m == NULL || (m->m_len < sizeof(*evl) &&
-			    (m = m_pullup(m, sizeof(*evl))) == NULL)) {
-				NG_FREE_ITEM(item);
-				return (ENOMEM);
+			eth_vtag = (hook_data & VLAN_TAG_MASK);
+			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
+				/* Just set packet header tag and send. */
+				m->m_flags |= M_VLANTAG;
+				m->m_pkthdr.ether_vtag = eth_vtag;
+				goto send_packet;
 			}
-			/*
-			 * Transform the Ethernet header into an Ethernet header
-			 * with 802.1Q encapsulation.
-			 */
-			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,
-			    mtod(m, char *), ETHER_HDR_LEN);
-			evl = mtod(m, struct ether_vlan_header *);
-			evl->evl_proto = evl->evl_encap_proto;
-			evl->evl_encap_proto = htons(ETHERTYPE_VLAN);
-			evl->evl_tag = htons(f->vlan);
+		} else { /* nomatch_hook */
+			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
+			    (m->m_flags & M_VLANTAG) == 0)
+				goto send_packet;
+			/* Encapsulate tagged packet. */
+			eth_vtag = m->m_pkthdr.ether_vtag;
+			m->m_pkthdr.ether_vtag = 0;
+			m->m_flags &= ~M_VLANTAG;
 		}
-		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);
+
+		/*
+		 * Transform the Ethernet header into an Ethernet header
+		 * with 802.1Q encapsulation.
+		 * Mod of: ether_vlanencap.
+		 *
+		 * TPID = ether type encap
+		 * Move DstMAC and SrcMAC from ETHER_TYPE.
+		 * Before:
+		 *  [free space ] [dmac] [smac] [ether_type] [payload]
+		 *  <<<<<<<<<<<<< |-----------| |--------------------|
+		 * After:
+		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
+		 *  |-----------| |-- inserted tag --| |--------------------|
+		 */
+		M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
+		if (m == NULL)
+			error = ENOMEM;
+		else
+			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
+		if (error != 0)
+			goto mchk_err;
+
+		evl = mtod(m, struct ether_vlan_header *);
+		bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
+		    (char *)evl, (ETHER_ADDR_LEN * 2));
+		evl->evl_encap_proto = priv->encap_proto;
+		evl->evl_tag = htons(eth_vtag);
 	}
+
+send_packet:
+	NG_FWD_NEW_DATA(error, item, dst_hook, m);
+	return (error);
+net_down:
+	error = ENETDOWN;
+drop:
+	m_freem(m);
+mchk_err:
+	NG_FREE_ITEM(item);
 	return (error);
 }
 
@@ -444,7 +690,7 @@ static int
 ng_vlan_disconnect(hook_p hook)
 {
 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
-	struct filter *f;
+	uintptr_t hook_data;
 
 	if (hook == priv->downstream_hook)
 		priv->downstream_hook = NULL;
@@ -452,11 +698,9 @@ ng_vlan_disconnect(hook_p hook)
 		priv->nomatch_hook = NULL;
 	else {
 		/* Purge a rule that refers to this hook. */
-		if ((f = NG_HOOK_PRIVATE(hook)) != NULL) {
-			LIST_REMOVE(f, next);
-			priv->nent--;
-			free(f, M_NETGRAPH);
-		}
+		hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+		if (IS_HOOK_VLAN_SET(hook_data))
+			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 	}
 	NG_HOOK_SET_PRIVATE(hook, NULL);
 	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&