2 files changed, 427 insertions, 150 deletions

diff --git a/sys/netgraph/ng_vlan.c b/sys/netgraph/ng_vlan.c
index f9de94fac669..b07a603a1b08 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) &&
diff --git a/sys/netgraph/ng_vlan.h b/sys/netgraph/ng_vlan.h
index 7cedc995bb7b..474238c83a0b 100644
--- a/sys/netgraph/ng_vlan.h
+++ b/sys/netgraph/ng_vlan.h
@@ -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
@@ -31,6 +32,9 @@
 #ifndef _NETGRAPH_NG_VLAN_H_
 #define	_NETGRAPH_NG_VLAN_H_
 
+/* Using "vlan" in addfilter and gettable messages. 2012.01 */
+#define	NG_VLAN_USE_OLD_VLAN_NAME 1
+
 /* Node type name and magic cookie. */
 #define	NG_VLAN_NODE_TYPE	"vlan"
 #define	NGM_VLAN_COOKIE		1068486472
@@ -43,21 +47,50 @@
 enum {
 	NGM_VLAN_ADD_FILTER = 1,
 	NGM_VLAN_DEL_FILTER,
-	NGM_VLAN_GET_TABLE
+	NGM_VLAN_GET_TABLE,
+	NGM_VLAN_DEL_VID_FLT,
+	NGM_VLAN_GET_DECAP,
+	NGM_VLAN_SET_DECAP,
+	NGM_VLAN_GET_ENCAP,
+	NGM_VLAN_SET_ENCAP,
+	NGM_VLAN_GET_ENCAP_PROTO,
+	NGM_VLAN_SET_ENCAP_PROTO,
 };
 
+#define	VLAN_ENCAP_FROM_FILTER	0x00000001
+#define	VLAN_ENCAP_FROM_NOMATCH	0x00000002
+
 /* For NGM_VLAN_ADD_FILTER control message. */
 struct ng_vlan_filter {
-	char		hook[NG_HOOKSIZ];
-	u_int16_t	vlan;
-};	
+	char		hook_name[NG_HOOKSIZ];
+#ifdef	NG_VLAN_USE_OLD_VLAN_NAME
+	uint16_t	vlan;	/* VLAN - same as vid, oldname, deprecated. */
+#endif
+	uint16_t	vid;	/* VID - VLAN Identifier. */
+	uint8_t		pcp;	/* PCP - Priority Code Point. */
+	uint8_t		cfi;	/* CFI - Canonical Format Indicator. */
+};
 
 /* Keep this in sync with the above structure definition.  */
+#ifdef	NG_VLAN_USE_OLD_VLAN_NAME
 #define	NG_VLAN_FILTER_FIELDS	{				\
-	{ "hook",	&ng_parse_hookbuf_type  },		\
-	{ "vlan",	&ng_parse_uint16_type   },		\
+	{ "hook",	&ng_parse_hookbuf_type	},		\
+	{ "vlan",	&ng_parse_uint16_type	},		\
+	{ "vid",	&ng_parse_uint16_type	},		\
+	{ "pcp",	&ng_parse_uint8_type	},		\
+	{ "cfi",	&ng_parse_uint8_type	},		\
 	{ NULL }						\
 }
+#else
+#define	NG_VLAN_FILTER_FIELDS	{				\
+	{ "hook",	&ng_parse_hookbuf_type	},		\
+	{ "vid",	&ng_parse_uint16_type	},		\
+	{ "pcp",	&ng_parse_uint8_type	},		\
+	{ "cfi",	&ng_parse_uint8_type	},		\
+	{ NULL }						\
+}
+#endif
+
 
 /* Structure returned by NGM_VLAN_GET_TABLE. */
 struct ng_vlan_table {