From nobody@FreeBSD.org  Sat Oct 22 19:54:18 2011
Return-Path: <nobody@FreeBSD.org>
Received: from mx1.freebsd.org (mx1.freebsd.org [IPv6:2001:4f8:fff6::34])
	by hub.freebsd.org (Postfix) with ESMTP id B2085106566B
	for <freebsd-gnats-submit@FreeBSD.org>; Sat, 22 Oct 2011 19:54:18 +0000 (UTC)
	(envelope-from nobody@FreeBSD.org)
Received: from red.freebsd.org (red.freebsd.org [IPv6:2001:4f8:fff6::22])
	by mx1.freebsd.org (Postfix) with ESMTP id A1A508FC0C
	for <freebsd-gnats-submit@FreeBSD.org>; Sat, 22 Oct 2011 19:54:18 +0000 (UTC)
Received: from red.freebsd.org (localhost [127.0.0.1])
	by red.freebsd.org (8.14.4/8.14.4) with ESMTP id p9MJsI55026338
	for <freebsd-gnats-submit@FreeBSD.org>; Sat, 22 Oct 2011 19:54:18 GMT
	(envelope-from nobody@red.freebsd.org)
Received: (from nobody@localhost)
	by red.freebsd.org (8.14.4/8.14.4/Submit) id p9MJsIUj026300;
	Sat, 22 Oct 2011 19:54:18 GMT
	(envelope-from nobody)
Message-Id: <201110221954.p9MJsIUj026300@red.freebsd.org>
Date: Sat, 22 Oct 2011 19:54:18 GMT
From: Ivan <rozhuk.im@gmail.com>
To: freebsd-gnats-submit@FreeBSD.org
Subject: ng_vlan update for QinQ support
X-Send-Pr-Version: www-3.1
X-GNATS-Notify:

>Number:         161908
>Category:       kern
>Synopsis:       [netgraph] [patch] ng_vlan update for QinQ support
>Confidential:   no
>Severity:       non-critical
>Priority:       medium
>Responsible:    melifaro
>State:          open
>Quarter:        
>Keywords:       
>Date-Required:  
>Class:          update
>Submitter-Id:   current-users
>Arrival-Date:   Sat Oct 22 20:00:28 UTC 2011
>Closed-Date:    
>Last-Modified:  Sun Mar 11 19:10:11 UTC 2012
>Originator:     Ivan
>Release:        FreeBSD 8.2-STABLE
>Organization:
>Environment:
FreeBSD firewall 8.2-STABLE FreeBSD 8.2-STABLE #16: Sat Oct 22 07:46:34 IRKST 2011     root@firewall:/usr/obj/usr/src/sys/RIMx64  amd64
>Description:
+ ethernet_type for VLAN encapsulation is tunable, default is: 0x8100 (33024)
+ PCP (Priority Code Point) and CFI (Canonical Format Indicator) for VLAN encapsulation is tunable per VID
+ VLAN filter can be deleted by VID
+ tunable encapsulation: on - do 802.1Q encapsulation, off - set M_VLANTAG and ether_vtag
* improved encapsulation/decapsulation code
* "vlan" changed to "vid" in "addfilter" and "gettable" messages
* many other canges


kldunload ng_vlan
kldload ng_ether

ngctl msg re0: setpromisc   1
ngctl msg re0: setautosrc   0

ngctl mkpeer re0: vlan lower downstream
ngctl connect re0: re0:lower upper nomatch

ngctl mkpeer re0:lower eiface vlan1001 ether
ngctl mkpeer re0:lower eiface vlan1002 ether

ngctl name re0:lower:vlan1001 ngeth0
ifconfig ngeth0 link 00:1a:4d:55:9a:43
ifconfig ngeth0 inet 192.168.0.36 netmask 255.255.255.0

ngctl name re0:lower:vlan1002 ngeth1
ifconfig ngeth1 link 00:1a:4d:55:9a:44
ifconfig ngeth1 inet 192.168.254.36 netmask 255.255.255.0

ngctl msg re0:lower addfilter '{ vid=1001 hook="vlan1001" }'
ngctl msg re0:lower addfilter '{ vid=1002 pcp=6 cfi=1 hook="vlan1002" }'



# ngctl msg re0:lower gettable
Rec'd response "gettable" (4) from "[156]:":
Args:   { n=2 filter=[ { hook="vlan1001" vid=1001 } { hook="vlan1002" vid=1002 pcp=6 cfi=1 } ] }

# ngctl msg re0:lower getencap
Rec'd response "getencap" (5) from "[156]:":
Args:   1

# ngctl msg re0:lower getencapproto
Rec'd response "getencapproto" (7) from "[156]:":
Args:   33024

# ngctl msg re0:lower delvidflt 1001

# ngctl msg re0:lower gettable
Rec'd response "gettable" (4) from "[156]:":
Args:   { n=1 filter=[ { hook="vlan1002" vid=1002 pcp=6 cfi=1 } ] }

# ngctl msg re0:lower delfilter '"vlan1002"'

# ngctl msg re0:lower gettable
Rec'd response "gettable" (4) from "[156]:":
Args:   {}



NOTE:
1.1 Use "setencap" = 0 with care: node connected to "downstream" must handle M_VLANTAG + ether_vtag. If it ng_ether, then IFCAP_VLAN_HWTAGGING must be enabled on attached network adapter.

1.2 Then "setencap" = 0, "setencapproto" value is ignored and assumed that 0x8100 (M_VLANTAG + ether_vtag - allways encapsulated with tag 0x8100)


2. "addfilter" syntax changed!!!
was:
ngctl msg re0:lower addfilter '{ vlan=1001 hook="vlan1001" }'

now:
ngctl msg re0:lower addfilter '{ vid=1001 hook="vlan1001" }'
ngctl msg re0:lower addfilter '{ vid=1001 hook="vlan1001" pcp=0 cfi=0 }'
(equivalent)


3. Trick:
kern.ipc.max_linkhdr should be increased via sysctl for best perfomance:
20 - 1 VLAN tag (.Q)
24 - 2 VLAN tags (QinQ)
28 - 3 VLAN tags (QinQinQ)
32 - 4 VLAN tags (...)

>How-To-Repeat:

>Fix:


Patch attached with submission follows:

--- /usr/src/sys/netgraph/ng_vlan.c.orig	2009-08-03 17:13:06.000000000 +0900
+++ /usr/src/sys/netgraph/ng_vlan.c	2011-10-23 03:41:00.000000000 +0900
@@ -1,5 +1,6 @@
 /*-
  * Copyright (c) 2003 IPNET Internet Communication Company
+ * Copyright (c) 2011 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;
+	int		encap_enable;
+	u_int16_t	encap_proto;
+	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];
+};
+typedef struct ng_vlan_private *priv_p;
+
+#define VLAN_TAG_MASK	0xFFFF
+#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
+#define IS_HOOK_VLAN_SET(hook_data) ((((uintptr_t)hook_data) & 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;
@@ -105,11 +122,46 @@
 	},
 	{
 	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_DEL_VID_FLT,
+	  "delvidflt",
+	  &ng_parse_int16_type,
+	  NULL
+	},
+	{
+	  NGM_VLAN_COOKIE,
 	  NGM_VLAN_GET_TABLE,
 	  "gettable",
 	  NULL,
 	  &ng_vlan_table_type
 	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_GET_ENCAP,
+	  "getencap",
+	  NULL,
+	  &ng_parse_int32_type
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_SET_ENCAP,
+	  "setencap",
+	  &ng_parse_int32_type,
+	  NULL
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_GET_ENCAP_PROTO,
+	  "getencapproto",
+	  NULL,
+	  &ng_parse_int32_type
+	},
+	{
+	  NGM_VLAN_COOKIE,
+	  NGM_VLAN_SET_ENCAP_PROTO,
+	  "setencapproto",
+	  &ng_parse_int32_type,
+	  NULL
+	},
 	{ 0 }
 };
 
@@ -126,47 +178,43 @@
 };
 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 STUFF
+//************************************************************************
 
-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;
+	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);
 
-	LIST_FOREACH(f, chain, next)
-		if (f->vlan == vlan)
-			return (f);
-	return (NULL);
+return (0);
 }
 
+//************************************************************************
+//			NETGRAPH NODE STUFF
+//************************************************************************
+
 static int
 ng_vlan_constructor(node_p node)
 {
 	priv_p priv;
-	int i;
 
 	priv = malloc(sizeof(*priv), M_NETGRAPH, M_NOWAIT | M_ZERO);
 	if (priv == NULL)
 		return (ENOMEM);
-	for (i = 0; i < HASHSIZE; i++)
-		LIST_INIT(&priv->hashtable[i]);
+	priv->encap_enable = 1;
+	priv->encap_proto = htons(ETHERTYPE_VLAN);
+	
 	NG_NODE_SET_PRIVATE(node, priv);
+
 	return (0);
 }
 
@@ -193,13 +241,14 @@
 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;
+	u_int16_t vid;
+	int error = 0;
 
 	NGI_GET_MSG(item, msg);
 	/* Deal with message according to cookie and command. */
@@ -214,12 +263,12 @@
 			}
 			vf = (struct ng_vlan_filter *)msg->data;
 			/* Sanity check the VLAN ID value. */
-			if (vf->vlan & ~EVL_VLID_MASK) {
+			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;
@@ -231,30 +280,18 @@
 				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. */
@@ -264,35 +301,125 @@
 			}
 			/* 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;
+			}
+#ifdef NETGRAPH_DEBUG
+			if (priv->vlan_hook[EVL_VLANOFTAG(hook_data)] != hook)
+				printf("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n", __func__, (char *)msg->data);
+#endif
+			/* 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(u_int16_t)) {
+				error = EINVAL;
+				break;
+			}
+			vid = (*((u_int16_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;
 			}
+#ifdef NETGRAPH_DEBUG
+			if (EVL_VLANOFTAG(hook_data) != vid)
+				printf("%s: NGM_VLAN_DEL_VID_FLT: Invalid VID Hook = %us, must be: %us\n", __func__, (u_int16_t)EVL_VLANOFTAG(hook_data), vid);
+#endif
 			/* 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),
+			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;
+#ifdef NETGRAPH_DEBUG
+				if (EVL_VLANOFTAG(hook_data) != i)
+					printf("%s: NGM_VLAN_GET_TABLE: hook %s VID = %us, must be: %i\n", __func__, NG_HOOK_NAME(hook), (u_int16_t)EVL_VLANOFTAG(hook_data), 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++;
-				}
+				vf ++;
+				t->n ++;
+			}
+			break;
+		case NGM_VLAN_GET_ENCAP:
+			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+			(*((u_int32_t *)resp->data)) = priv->encap_enable;
+			break;
+		case NGM_VLAN_SET_ENCAP:
+			if (msg->header.arglen != sizeof(u_int32_t)) {
+				error = EINVAL;
+				break;
+			}
+			priv->encap_enable = ((*((u_int32_t *)msg->data)) != 0);
+			break;
+		case NGM_VLAN_GET_ENCAP_PROTO:
+			NG_MKRESPONSE(resp, msg, sizeof(u_int16_t), M_NOWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+			(*((u_int16_t *)resp->data)) = ntohs(priv->encap_proto);
+			break;
+		case NGM_VLAN_SET_ENCAP_PROTO:
+			if (msg->header.arglen != sizeof(u_int16_t)) {
+				error = EINVAL;
+				break;
 			}
+			priv->encap_proto = htons((*((u_int16_t *)msg->data)));
 			break;
 		default:		/* Unknown command. */
 			error = EINVAL;
@@ -302,8 +429,6 @@
 	case NGM_FLOW_COOKIE:
 	    {
 		struct ng_mesg *copy;
-		struct filterhead *chain;
-		struct filter *f;
 
 		/*
 		 * Flow control messages should come only
@@ -314,17 +439,16 @@
 			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);
+					continue;
+			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);
 		}
-
 		break;
 	    }
 	default:			/* Unknown type cookie. */
@@ -343,16 +467,19 @@
 	struct ether_header *eh;
 	struct ether_vlan_header *evl = NULL;
 	int error;
-	u_int16_t vlan;
+	uintptr_t hook_data;
+	u_int16_t vid;
 	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) {
+
+	/* Make sure we have an entire header. */
+	error = m_chk(&m, ETHER_HDR_LEN);
+	if (error != 0) {
 		NG_FREE_ITEM(item);
-		return (EINVAL);
+		return (error);
 	}
 	eh = mtod(m, struct ether_header *);
 	if (hook == priv->downstream_hook) {
@@ -360,75 +487,104 @@
 		 * If from downstream, select between a match hook
 		 * or the nomatch hook.
 		 */
+		dst_hook = priv->nomatch_hook;
 		if (m->m_flags & M_VLANTAG ||
-		    eh->ether_type == htons(ETHERTYPE_VLAN)) {
+		    eh->ether_type == priv->encap_proto) {
 			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) {
+				vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
+			} else { /* eh->ether_type == priv->encap_proto */
+				error = m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));
+				if (error != 0) {
 					NG_FREE_ITEM(item);
-					return (EINVAL);
+					return (error);
 				}
 				evl = mtod(m, struct ether_vlan_header *);
-				vlan = EVL_VLANOFTAG(ntohs(evl->evl_tag));
+				vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
 			}
-			if ((f = ng_vlan_findentry(priv, vlan)) != NULL) {
+
+			if (priv->vlan_hook[vid] != NULL) {
+				dst_hook = priv->vlan_hook[vid];
 				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);
+					/* 
+					 * move DstMAC and SrcMAC to ETHER_TYPE
+					 * before: [dst_mac] [src_mac] [ether_type_encap(TPID)] [PCP/CFI/VID] [ether_type] [payload]
+					 *         |-----------------| >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |---------------------|
+					 * after: [free space] [dst_mac] [src_mac] [ether_type] [payload]
+					 *                     |-----------------| |---------------------|
+					 */
+					bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
+						(ETHER_ADDR_LEN * 2));
 					m_adj(m, ETHER_VLAN_ENCAP_LEN);
 				}
 			}
-		} 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 {
 		/*
 		 * 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) {
+		dst_hook = priv->downstream_hook;
+		if (dst_hook != NULL && hook != priv->nomatch_hook) {
+			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
+			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
+				m_freem(m);
 				NG_FREE_ITEM(item);
-				NG_FREE_M(m);
 				return (EOPNOTSUPP);
 			}
-			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);
+			if (priv->encap_enable == 0) {
+				/* just set packet header tag */
+				m->m_flags |= M_VLANTAG;
+				m->m_pkthdr.ether_vtag = (hook_data & VLAN_TAG_MASK);
+			} else {
+				/*
+				 * Transform the Ethernet header into an Ethernet header
+				 * with 802.1Q encapsulation.
+				 * mod of: ether_vlanencap 
+				 */
+				M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
+				/* M_PREPEND takes care of m_len and m_pkthdr.len. */
+				if (m == NULL)
+					error = ENOMEM;
+				else
+					error = m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));
+				if (error != 0) {
+					NG_FREE_ITEM(item);
+					return (error);
+				}
+				/* move DstMAC and SrcMAC from ETHER_TYPE
+				 * before: [free - prepended space] [dst_mac] [src_mac] [ether_type] [payload]
+				 *          <<<<<<<<<<<<<<<<<<<<<<  |-----------------| |--------------------|
+				 * after: [dst_mac] [src_mac] [ether_type_encap(TPID)] [PCP/CFI/VID] [ether_type] [payload]
+				 *        |-----------------| |----------- inserted tag -----------| |--------------------| 
+				 */
+				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((hook_data & VLAN_TAG_MASK));
 			}
-			/*
-			 * 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);
 		}
-		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);
 	}
-	return (error);
+
+	/* send packet */
+	if (dst_hook != NULL) {
+		NG_FWD_NEW_DATA(error, item, dst_hook, m);
+		return (error);
+	}
+
+	/* no hook to send */
+	m_freem(m);
+	NG_FREE_ITEM(item);
+
+	return (ENETDOWN);
 }
 
 static int
@@ -446,7 +602,7 @@
 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;
@@ -454,11 +610,9 @@
 		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) == 0)
+			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 	}
 	NG_HOOK_SET_PRIVATE(hook, NULL);
 	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&


--- /usr/src/sys/netgraph/ng_vlan.h.orig	2009-08-03 17:13:06.000000000 +0900
+++ /usr/src/sys/netgraph/ng_vlan.h	2011-10-22 19:11:01.000000000 +0900
@@ -1,5 +1,6 @@
 /*-
  * Copyright (c) 2003 IPNET Internet Communication Company
+ * Copyright (c) 2011 Rozhuk Ivan <rozhuk.im@gmail.com>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -43,19 +44,28 @@
 enum {
 	NGM_VLAN_ADD_FILTER = 1,
 	NGM_VLAN_DEL_FILTER,
-	NGM_VLAN_GET_TABLE
+	NGM_VLAN_DEL_VID_FLT,
+	NGM_VLAN_GET_TABLE,
+	NGM_VLAN_GET_ENCAP,
+	NGM_VLAN_SET_ENCAP,
+	NGM_VLAN_GET_ENCAP_PROTO,
+	NGM_VLAN_SET_ENCAP_PROTO,
 };
 
 /* For NGM_VLAN_ADD_FILTER control message. */
 struct ng_vlan_filter {
-	char		hook[NG_HOOKSIZ];
-	u_int16_t	vlan;
-};	
+	char		hook_name[NG_HOOKSIZ];
+	u_int16_t	vid;	/* VID - VLAN Identifier */
+	u_int8_t	pcp;	/* PCP - Priority Code Point */
+	u_int8_t	cfi;	/* CFI - Canonical Format Indicator */
+};
 
 /* Keep this in sync with the above structure definition.  */
 #define	NG_VLAN_FILTER_FIELDS	{				\
-	{ "hook",	&ng_parse_hookbuf_type  },		\
-	{ "vlan",	&ng_parse_uint16_type   },		\
+	{ "hook",	&ng_parse_hookbuf_type	},		\
+	{ "vid",	&ng_parse_uint16_type	},		\
+	{ "pcp",	&ng_parse_uint8_type	},		\
+	{ "cfi",	&ng_parse_uint8_type	},		\
 	{ NULL }						\
 }
 


>Release-Note:
>Audit-Trail:

From: rozhuk.im@gmail.com
To: <bug-followup@FreeBSD.org>,
	<rozhuk.im@gmail.com>
Cc:  
Subject: Re: kern/161908: ng_vlan update for QinQ support
Date: Sun, 23 Oct 2011 08:01:20 +0900

 This is a multi-part message in MIME format.
 
 ------=_NextPart_000_02C1_01CC9159.F56007D0
 Content-Type: text/plain;
 	charset="windows-1251"
 Content-Transfer-Encoding: quoted-printable
 
 		/* Purge a rule that refers to this hook. */
 		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);
 		if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)
 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;
 
 should be:
 
 		/* Purge a rule that refers to this hook. */
 		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);
 		if (IS_HOOK_VLAN_SET(hook_data))
 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;
 
 =A0
 --
 Rozhuk Ivan
 =A0=20
 
 
 
 ------=_NextPart_000_02C1_01CC9159.F56007D0
 Content-Type: text/plain;
 	name="ng_vlan.patch.txt"
 Content-Transfer-Encoding: quoted-printable
 Content-Disposition: attachment;
 	filename="ng_vlan.patch.txt"
 
 --- /usr/src/sys/netgraph/ng_vlan.c.orig	2009-08-03 17:13:06.000000000 =
 +0900=0A=
 +++ /usr/src/sys/netgraph/ng_vlan.c	2011-10-23 03:41:00.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -46,6 +47,22 @@=0A=
  #include <netgraph/ng_vlan.h>=0A=
  #include <netgraph/netgraph.h>=0A=
  =0A=
 +=0A=
 +=0A=
 +struct ng_vlan_private {=0A=
 +	hook_p		downstream_hook;=0A=
 +	hook_p		nomatch_hook;=0A=
 +	int		encap_enable;=0A=
 +	u_int16_t	encap_proto;=0A=
 +	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];=0A=
 +};=0A=
 +typedef struct ng_vlan_private *priv_p;=0A=
 +=0A=
 +#define VLAN_TAG_MASK	0xFFFF=0A=
 +#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))=0A=
 +#define IS_HOOK_VLAN_SET(hook_data) ((((uintptr_t)hook_data) & =
 HOOK_VLAN_TAG_SET_MASK) =3D=3D HOOK_VLAN_TAG_SET_MASK)=0A=
 +=0A=
 +=0A=
  static ng_constructor_t	ng_vlan_constructor;=0A=
  static ng_rcvmsg_t	ng_vlan_rcvmsg;=0A=
  static ng_shutdown_t	ng_vlan_shutdown;=0A=
 @@ -105,11 +122,46 @@=0A=
  	},=0A=
  	{=0A=
  	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_DEL_VID_FLT,=0A=
 +	  "delvidflt",=0A=
 +	  &ng_parse_int16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
  	  NGM_VLAN_GET_TABLE,=0A=
  	  "gettable",=0A=
  	  NULL,=0A=
  	  &ng_vlan_table_type=0A=
  	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP,=0A=
 +	  "getencap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_int32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP,=0A=
 +	  "setencap",=0A=
 +	  &ng_parse_int32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	  "getencapproto",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_int32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP_PROTO,=0A=
 +	  "setencapproto",=0A=
 +	  &ng_parse_int32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
  	{ 0 }=0A=
  };=0A=
  =0A=
 @@ -126,47 +178,43 @@=0A=
  };=0A=
  NETGRAPH_INIT(vlan, &ng_vlan_typestruct);=0A=
  =0A=
 -struct filter {=0A=
 -	LIST_ENTRY(filter) next;=0A=
 -	u_int16_t	vlan;=0A=
 -	hook_p		hook;=0A=
 -};=0A=
  =0A=
 -#define	HASHSIZE	16=0A=
 -#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)=0A=
 -LIST_HEAD(filterhead, filter);=0A=
  =0A=
 -typedef struct {=0A=
 -	hook_p		downstream_hook;=0A=
 -	hook_p		nomatch_hook;=0A=
 -	struct filterhead hashtable[HASHSIZE];=0A=
 -	u_int32_t	nent;=0A=
 -} *priv_p;=0A=
 +//**********************************************************************=
 **=0A=
 +//			HELPER STUFF=0A=
 +//**********************************************************************=
 **=0A=
  =0A=
 -static struct filter *=0A=
 -ng_vlan_findentry(priv_p priv, u_int16_t vlan)=0A=
 +static __inline int=0A=
 +m_chk(struct mbuf **mp, int len)=0A=
  {=0A=
 -	struct filterhead *chain =3D &priv->hashtable[HASH(vlan)];=0A=
 -	struct filter *f;=0A=
 +	if ((*mp)->m_pkthdr.len < len) {=0A=
 +		m_freem((*mp));=0A=
 +		(*mp) =3D NULL;=0A=
 +		return (EINVAL);=0A=
 +	}=0A=
 +	if ((*mp)->m_len < len && ((*mp) =3D m_pullup((*mp), len)) =3D=3D NULL)=0A=
 +		return (ENOBUFS);=0A=
  =0A=
 -	LIST_FOREACH(f, chain, next)=0A=
 -		if (f->vlan =3D=3D vlan)=0A=
 -			return (f);=0A=
 -	return (NULL);=0A=
 +return (0);=0A=
  }=0A=
  =0A=
 +//**********************************************************************=
 **=0A=
 +//			NETGRAPH NODE STUFF=0A=
 +//**********************************************************************=
 **=0A=
 +=0A=
  static int=0A=
  ng_vlan_constructor(node_p node)=0A=
  {=0A=
  	priv_p priv;=0A=
 -	int i;=0A=
  =0A=
  	priv =3D malloc(sizeof(*priv), M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
  	if (priv =3D=3D NULL)=0A=
  		return (ENOMEM);=0A=
 -	for (i =3D 0; i < HASHSIZE; i++)=0A=
 -		LIST_INIT(&priv->hashtable[i]);=0A=
 +	priv->encap_enable =3D 1;=0A=
 +	priv->encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 +	=0A=
  	NG_NODE_SET_PRIVATE(node, priv);=0A=
 +=0A=
  	return (0);=0A=
  }=0A=
  =0A=
 @@ -193,13 +241,14 @@=0A=
  ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(node);=0A=
 -	int error =3D 0;=0A=
  	struct ng_mesg *msg, *resp =3D NULL;=0A=
  	struct ng_vlan_filter *vf;=0A=
 -	struct filter *f;=0A=
  	hook_p hook;=0A=
  	struct ng_vlan_table *t;=0A=
 -	int i;=0A=
 +	uintptr_t hook_data;=0A=
 +	int i, vlan_count;=0A=
 +	u_int16_t vid;=0A=
 +	int error =3D 0;=0A=
  =0A=
  	NGI_GET_MSG(item, msg);=0A=
  	/* Deal with message according to cookie and command. */=0A=
 @@ -214,12 +263,12 @@=0A=
  			}=0A=
  			vf =3D (struct ng_vlan_filter *)msg->data;=0A=
  			/* Sanity check the VLAN ID value. */=0A=
 -			if (vf->vlan & ~EVL_VLID_MASK) {=0A=
 +			if (vf->vid & ~EVL_VLID_MASK || vf->pcp & ~7 || vf->cfi & ~1) {=0A=
  				error =3D EINVAL;=0A=
  				break;=0A=
  			}=0A=
  			/* Check that a referenced hook exists. */=0A=
 -			hook =3D ng_findhook(node, vf->hook);=0A=
 +			hook =3D ng_findhook(node, vf->hook_name);=0A=
  			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
 @@ -231,30 +280,18 @@=0A=
  				break;=0A=
  			}=0A=
  			/* And is not already in service. */=0A=
 -			if (NG_HOOK_PRIVATE(hook) !=3D NULL) {=0A=
 +			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
  			/* Check we don't already trap this VLAN. */=0A=
 -			if (ng_vlan_findentry(priv, vf->vlan)) {=0A=
 +			if (priv->vlan_hook[vf->vid] !=3D NULL) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
 -			/* Create filter. */=0A=
 -			f =3D malloc(sizeof(*f),=0A=
 -			    M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
 -			if (f =3D=3D NULL) {=0A=
 -				error =3D ENOMEM;=0A=
 -				break;=0A=
 -			}=0A=
 -			/* Link filter and hook together. */=0A=
 -			f->hook =3D hook;=0A=
 -			f->vlan =3D vf->vlan;=0A=
 -			NG_HOOK_SET_PRIVATE(hook, f);=0A=
 -			/* Register filter in a hash table. */=0A=
 -			LIST_INSERT_HEAD(=0A=
 -			    &priv->hashtable[HASH(f->vlan)], f, next);=0A=
 -			priv->nent++;=0A=
 +			/* Link vlan and hook together. */=0A=
 +			NG_HOOK_SET_PRIVATE(hook, (void *)(HOOK_VLAN_TAG_SET_MASK | =
 EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));=0A=
 +			priv->vlan_hook[vf->vid] =3D hook;=0A=
  			break;=0A=
  		case NGM_VLAN_DEL_FILTER:=0A=
  			/* Check that message is long enough. */=0A=
 @@ -264,35 +301,125 @@=0A=
  			}=0A=
  			/* Check that hook exists and is active. */=0A=
  			hook =3D ng_findhook(node, (char *)msg->data);=0A=
 -			if (hook =3D=3D NULL ||=0A=
 -			    (f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (priv->vlan_hook[EVL_VLANOFTAG(hook_data)] !=3D hook)=0A=
 +				printf("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook =3D %s\n", =
 __func__, (char *)msg->data);=0A=
 +#endif=0A=
 +			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
 +			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 +			break;=0A=
 +		case NGM_VLAN_DEL_VID_FLT:=0A=
 +			/* Check that message is long enough. */=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			vid =3D (*((u_int16_t *)msg->data));=0A=
 +			/* Sanity check the VLAN ID value. */=0A=
 +			if (vid & ~EVL_VLID_MASK) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			/* Check that hook exists and is active. */=0A=
 +			hook =3D priv->vlan_hook[vid];=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
  			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (EVL_VLANOFTAG(hook_data) !=3D vid)=0A=
 +				printf("%s: NGM_VLAN_DEL_VID_FLT: Invalid VID Hook =3D %us, must =
 be: %us\n", __func__, (u_int16_t)EVL_VLANOFTAG(hook_data), vid);=0A=
 +#endif=0A=
  			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[vid] =3D NULL;=0A=
  			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
  			break;=0A=
  		case NGM_VLAN_GET_TABLE:=0A=
 +			/* calculate vlans */=0A=
 +			vlan_count =3D 0;=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				if (priv->vlan_hook[i] !=3D NULL=0A=
 +				   && NG_HOOK_IS_VALID(priv->vlan_hook[i]))=0A=
 +					vlan_count ++;=0A=
 +			}=0A=
 +=0A=
 +			/* allocate memory for responce */=0A=
  			NG_MKRESPONSE(resp, msg, sizeof(*t) +=0A=
 -			    priv->nent * sizeof(*t->filter), M_NOWAIT);=0A=
 +			    vlan_count * sizeof(*t->filter), M_NOWAIT);=0A=
  			if (resp =3D=3D NULL) {=0A=
  				error =3D ENOMEM;=0A=
  				break;=0A=
  			}=0A=
 +=0A=
 +			/* pack data to responce */=0A=
  			t =3D (struct ng_vlan_table *)resp->data;=0A=
 -			t->n =3D priv->nent;=0A=
 +			t->n =3D 0;=0A=
  			vf =3D &t->filter[0];=0A=
 -			for (i =3D 0; i < HASHSIZE; i++) {=0A=
 -				LIST_FOREACH(f, &priv->hashtable[i], next) {=0A=
 -					vf->vlan =3D f->vlan;=0A=
 -					strncpy(vf->hook, NG_HOOK_NAME(f->hook),=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				hook =3D priv->vlan_hook[i];=0A=
 +				if (hook =3D=3D NULL=0A=
 +				   || NG_HOOK_NOT_VALID(hook))=0A=
 +					continue;=0A=
 +				hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +				if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)=0A=
 +					continue;=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +				if (EVL_VLANOFTAG(hook_data) !=3D i)=0A=
 +					printf("%s: NGM_VLAN_GET_TABLE: hook %s VID =3D %us, must be: =
 %i\n", __func__, NG_HOOK_NAME(hook), =
 (u_int16_t)EVL_VLANOFTAG(hook_data), i);=0A=
 +#endif=0A=
 +				vf->vid =3D i;=0A=
 +				vf->pcp =3D EVL_PRIOFTAG(hook_data);=0A=
 +				vf->cfi =3D EVL_CFIOFTAG(hook_data);=0A=
 +				strncpy(vf->hook_name, NG_HOOK_NAME(hook),=0A=
  					    NG_HOOKSIZ);=0A=
 -					vf++;=0A=
 -				}=0A=
 +				vf ++;=0A=
 +				t->n ++;=0A=
 +			}=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->encap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_enable =3D ((*((u_int32_t *)msg->data)) !=3D 0);=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP_PROTO:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int16_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int16_t *)resp->data)) =3D ntohs(priv->encap_proto);=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP_PROTO:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
  			}=0A=
 +			priv->encap_proto =3D htons((*((u_int16_t *)msg->data)));=0A=
  			break;=0A=
  		default:		/* Unknown command. */=0A=
  			error =3D EINVAL;=0A=
 @@ -302,8 +429,6 @@=0A=
  	case NGM_FLOW_COOKIE:=0A=
  	    {=0A=
  		struct ng_mesg *copy;=0A=
 -		struct filterhead *chain;=0A=
 -		struct filter *f;=0A=
  =0A=
  		/*=0A=
  		 * Flow control messages should come only=0A=
 @@ -314,17 +439,16 @@=0A=
  			break;=0A=
  		if (lasthook !=3D priv->downstream_hook)=0A=
  			break;=0A=
 -=0A=
  		/* Broadcast the event to all uplinks. */=0A=
 -		for (i =3D 0, chain =3D priv->hashtable; i < HASHSIZE;=0A=
 -		    i++, chain++)=0A=
 -		LIST_FOREACH(f, chain, next) {=0A=
 +		for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +			if (priv->vlan_hook[i] =3D=3D NULL)=0A=
 +				continue;=0A=
 +=0A=
  			NG_COPYMESSAGE(copy, msg, M_NOWAIT);=0A=
  			if (copy =3D=3D NULL)=0A=
 -				continue;=0A=
 -			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);=0A=
 +					continue;=0A=
 +			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);=0A=
  		}=0A=
 -=0A=
  		break;=0A=
  	    }=0A=
  	default:			/* Unknown type cookie. */=0A=
 @@ -343,16 +467,19 @@=0A=
  	struct ether_header *eh;=0A=
  	struct ether_vlan_header *evl =3D NULL;=0A=
  	int error;=0A=
 -	u_int16_t vlan;=0A=
 +	uintptr_t hook_data;=0A=
 +	u_int16_t vid;=0A=
  	struct mbuf *m;=0A=
 -	struct filter *f;=0A=
 +	hook_p dst_hook;=0A=
 +=0A=
  =0A=
 -	/* Make sure we have an entire header. */=0A=
  	NGI_GET_M(item, m);=0A=
 -	if (m->m_len < sizeof(*eh) &&=0A=
 -	    (m =3D m_pullup(m, sizeof(*eh))) =3D=3D NULL) {=0A=
 +=0A=
 +	/* Make sure we have an entire header. */=0A=
 +	error =3D m_chk(&m, ETHER_HDR_LEN);=0A=
 +	if (error !=3D 0) {=0A=
  		NG_FREE_ITEM(item);=0A=
 -		return (EINVAL);=0A=
 +		return (error);=0A=
  	}=0A=
  	eh =3D mtod(m, struct ether_header *);=0A=
  	if (hook =3D=3D priv->downstream_hook) {=0A=
 @@ -360,75 +487,104 @@=0A=
  		 * If from downstream, select between a match hook=0A=
  		 * or the nomatch hook.=0A=
  		 */=0A=
 +		dst_hook =3D priv->nomatch_hook;=0A=
  		if (m->m_flags & M_VLANTAG ||=0A=
 -		    eh->ether_type =3D=3D htons(ETHERTYPE_VLAN)) {=0A=
 +		    eh->ether_type =3D=3D priv->encap_proto) {=0A=
  			if (m->m_flags & M_VLANTAG) {=0A=
  				/*=0A=
  				 * Packet is tagged, m contains a normal=0A=
  				 * Ethernet frame; tag is stored out-of-band.=0A=
  				 */=0A=
 -				vlan =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 -			} else {=0A=
 -				if (m->m_len < sizeof(*evl) &&=0A=
 -				    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL) {=0A=
 +				vid =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 +			} else { /* eh->ether_type =3D=3D priv->encap_proto */=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0) {=0A=
  					NG_FREE_ITEM(item);=0A=
 -					return (EINVAL);=0A=
 +					return (error);=0A=
  				}=0A=
  				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -				vlan =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 +				vid =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
  			}=0A=
 -			if ((f =3D ng_vlan_findentry(priv, vlan)) !=3D NULL) {=0A=
 +=0A=
 +			if (priv->vlan_hook[vid] !=3D NULL) {=0A=
 +				dst_hook =3D priv->vlan_hook[vid];=0A=
  				if (m->m_flags & M_VLANTAG) {=0A=
  					m->m_pkthdr.ether_vtag =3D 0;=0A=
  					m->m_flags &=3D ~M_VLANTAG;=0A=
  				} else {=0A=
 -					evl->evl_encap_proto =3D evl->evl_proto;=0A=
 -					bcopy(mtod(m, caddr_t),=0A=
 -					    mtod(m, caddr_t) +=0A=
 -					    ETHER_VLAN_ENCAP_LEN,=0A=
 -					    ETHER_HDR_LEN);=0A=
 +					/* =0A=
 +					 * move DstMAC and SrcMAC to ETHER_TYPE=0A=
 +					 * before: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +					 *         |-----------------| =
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |---------------------|=0A=
 +					 * after: [free space] [dst_mac] [src_mac] [ether_type] [payload]=0A=
 +					 *                     |-----------------| |---------------------|=0A=
 +					 */=0A=
 +					bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +						(ETHER_ADDR_LEN * 2));=0A=
  					m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
  				}=0A=
  			}=0A=
 -		} else=0A=
 -			f =3D NULL;=0A=
 -		if (f !=3D NULL)=0A=
 -			NG_FWD_NEW_DATA(error, item, f->hook, m);=0A=
 -		else=0A=
 -			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);=0A=
 +		}=0A=
  	} else {=0A=
  		/*=0A=
  		 * It is heading towards the downstream.=0A=
  		 * If from nomatch, pass it unmodified.=0A=
  		 * Otherwise, do the VLAN encapsulation.=0A=
  		 */=0A=
 -		if (hook !=3D priv->nomatch_hook) {=0A=
 -			if ((f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +		dst_hook =3D priv->downstream_hook;=0A=
 +		if (dst_hook !=3D NULL && hook !=3D priv->nomatch_hook) {=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				m_freem(m);=0A=
  				NG_FREE_ITEM(item);=0A=
 -				NG_FREE_M(m);=0A=
  				return (EOPNOTSUPP);=0A=
  			}=0A=
 -			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 -			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 -			if (m =3D=3D NULL || (m->m_len < sizeof(*evl) &&=0A=
 -			    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL)) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				return (ENOMEM);=0A=
 +			if (priv->encap_enable =3D=3D 0) {=0A=
 +				/* just set packet header tag */=0A=
 +				m->m_flags |=3D M_VLANTAG;=0A=
 +				m->m_pkthdr.ether_vtag =3D (hook_data & VLAN_TAG_MASK);=0A=
 +			} else {=0A=
 +				/*=0A=
 +				 * Transform the Ethernet header into an Ethernet header=0A=
 +				 * with 802.1Q encapsulation.=0A=
 +				 * mod of: ether_vlanencap =0A=
 +				 */=0A=
 +				M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 +				/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 +				if (m =3D=3D NULL)=0A=
 +					error =3D ENOMEM;=0A=
 +				else=0A=
 +					error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0) {=0A=
 +					NG_FREE_ITEM(item);=0A=
 +					return (error);=0A=
 +				}=0A=
 +				/* move DstMAC and SrcMAC from ETHER_TYPE=0A=
 +				 * before: [free - prepended space] [dst_mac] [src_mac] =
 [ether_type] [payload]=0A=
 +				 *          <<<<<<<<<<<<<<<<<<<<<<  |-----------------| =
 |--------------------|=0A=
 +				 * after: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +				 *        |-----------------| |----------- inserted tag =
 -----------| |--------------------| =0A=
 +				 */=0A=
 +				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 +				bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +					(char *)evl, (ETHER_ADDR_LEN * 2));=0A=
 +				evl->evl_encap_proto =3D priv->encap_proto;=0A=
 +				evl->evl_tag =3D htons((hook_data & VLAN_TAG_MASK));=0A=
  			}=0A=
 -			/*=0A=
 -			 * Transform the Ethernet header into an Ethernet header=0A=
 -			 * with 802.1Q encapsulation.=0A=
 -			 */=0A=
 -			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,=0A=
 -			    mtod(m, char *), ETHER_HDR_LEN);=0A=
 -			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -			evl->evl_proto =3D evl->evl_encap_proto;=0A=
 -			evl->evl_encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 -			evl->evl_tag =3D htons(f->vlan);=0A=
  		}=0A=
 -		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);=0A=
  	}=0A=
 -	return (error);=0A=
 +=0A=
 +	/* send packet */=0A=
 +	if (dst_hook !=3D NULL) {=0A=
 +		NG_FWD_NEW_DATA(error, item, dst_hook, m);=0A=
 +		return (error);=0A=
 +	}=0A=
 +=0A=
 +	/* no hook to send */=0A=
 +	m_freem(m);=0A=
 +	NG_FREE_ITEM(item);=0A=
 +=0A=
 +	return (ENETDOWN);=0A=
  }=0A=
  =0A=
  static int=0A=
 @@ -446,7 +602,7 @@=0A=
  ng_vlan_disconnect(hook_p hook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
 -	struct filter *f;=0A=
 +	uintptr_t hook_data;=0A=
  =0A=
  	if (hook =3D=3D priv->downstream_hook)=0A=
  		priv->downstream_hook =3D NULL;=0A=
 @@ -454,11 +610,9 @@=0A=
  		priv->nomatch_hook =3D NULL;=0A=
  	else {=0A=
  		/* Purge a rule that refers to this hook. */=0A=
 -		if ((f =3D NG_HOOK_PRIVATE(hook)) !=3D NULL) {=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
 -		}=0A=
 +		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +		if (IS_HOOK_VLAN_SET(hook_data))=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
  	}=0A=
  	NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
  	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) =3D=3D 0) &&=0A=
 =0A=
 =0A=
 --- /usr/src/sys/netgraph/ng_vlan.h.orig	2009-08-03 17:13:06.000000000 =
 +0900=0A=
 +++ /usr/src/sys/netgraph/ng_vlan.h	2011-10-22 19:11:01.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -43,19 +44,28 @@=0A=
  enum {=0A=
  	NGM_VLAN_ADD_FILTER =3D 1,=0A=
  	NGM_VLAN_DEL_FILTER,=0A=
 -	NGM_VLAN_GET_TABLE=0A=
 +	NGM_VLAN_DEL_VID_FLT,=0A=
 +	NGM_VLAN_GET_TABLE,=0A=
 +	NGM_VLAN_GET_ENCAP,=0A=
 +	NGM_VLAN_SET_ENCAP,=0A=
 +	NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	NGM_VLAN_SET_ENCAP_PROTO,=0A=
  };=0A=
  =0A=
  /* For NGM_VLAN_ADD_FILTER control message. */=0A=
  struct ng_vlan_filter {=0A=
 -	char		hook[NG_HOOKSIZ];=0A=
 -	u_int16_t	vlan;=0A=
 -};	=0A=
 +	char		hook_name[NG_HOOKSIZ];=0A=
 +	u_int16_t	vid;	/* VID - VLAN Identifier */=0A=
 +	u_int8_t	pcp;	/* PCP - Priority Code Point */=0A=
 +	u_int8_t	cfi;	/* CFI - Canonical Format Indicator */=0A=
 +};=0A=
  =0A=
  /* Keep this in sync with the above structure definition.  */=0A=
  #define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 -	{ "hook",	&ng_parse_hookbuf_type  },		\=0A=
 -	{ "vlan",	&ng_parse_uint16_type   },		\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
  	{ NULL }						\=0A=
  }=0A=
  =0A=
 
 ------=_NextPart_000_02C1_01CC9159.F56007D0--
 
Responsible-Changed-From-To: freebsd-bugs->freebsd-net 
Responsible-Changed-By: eadler 
Responsible-Changed-When: Sun Oct 23 22:34:17 UTC 2011 
Responsible-Changed-Why:  
assign and add tags 

http://www.freebsd.org/cgi/query-pr.cgi?pr=161908 

From: rozhuk.im@gmail.com
To: <bug-followup@FreeBSD.org>,
	<rozhuk.im@gmail.com>
Cc:  
Subject: Re: kern/161908: [netgraph] [patch] ng_vlan update for QinQ support
Date: Tue, 25 Oct 2011 21:38:40 +0900

 This is a multi-part message in MIME format.
 
 ------=_NextPart_000_033B_01CC935E.7927C480
 Content-Type: text/plain;
 	charset="windows-1251"
 Content-Transfer-Encoding: quoted-printable
 
 
 Fixed ng_parce_types in struct ng_cmdlist ng_vlan_cmdlist:
 
 "delvidflt": ng_parse_int16_type -> ng_parse_uint16_type
 "getencap": ng_parse_int32_type -> ng_parse_uint32_type
 "setencap": ng_parse_int32_type -> ng_parse_uint32_type
 "getencapproto": ng_parse_int32_type -> ng_parse_uint16_type
 "setencapproto": ng_parse_int32_type -> ng_parse_uint16_type
 
 
 
 --
 Rozhuk Ivan
 =A0=20
 
 
 
 ------=_NextPart_000_033B_01CC935E.7927C480
 Content-Type: application/octet-stream;
 	name="ng_vlan.patch"
 Content-Transfer-Encoding: quoted-printable
 Content-Disposition: attachment;
 	filename="ng_vlan.patch"
 
 --- /usr/src/sys/netgraph/ng_vlan.c.orig	2009-08-03 17:13:06.000000000 =
 +0900=0A=
 +++ /usr/src/sys/netgraph/ng_vlan.c	2011-10-23 03:41:00.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -46,6 +47,22 @@=0A=
  #include <netgraph/ng_vlan.h>=0A=
  #include <netgraph/netgraph.h>=0A=
  =0A=
 +=0A=
 +=0A=
 +struct ng_vlan_private {=0A=
 +	hook_p		downstream_hook;=0A=
 +	hook_p		nomatch_hook;=0A=
 +	int		encap_enable;=0A=
 +	u_int16_t	encap_proto;=0A=
 +	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];=0A=
 +};=0A=
 +typedef struct ng_vlan_private *priv_p;=0A=
 +=0A=
 +#define VLAN_TAG_MASK	0xFFFF=0A=
 +#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))=0A=
 +#define IS_HOOK_VLAN_SET(hook_data) ((((uintptr_t)hook_data) & =
 HOOK_VLAN_TAG_SET_MASK) =3D=3D HOOK_VLAN_TAG_SET_MASK)=0A=
 +=0A=
 +=0A=
  static ng_constructor_t	ng_vlan_constructor;=0A=
  static ng_rcvmsg_t	ng_vlan_rcvmsg;=0A=
  static ng_shutdown_t	ng_vlan_shutdown;=0A=
 @@ -105,11 +122,46 @@=0A=
  	},=0A=
  	{=0A=
  	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_DEL_VID_FLT,=0A=
 +	  "delvidflt",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
  	  NGM_VLAN_GET_TABLE,=0A=
  	  "gettable",=0A=
  	  NULL,=0A=
  	  &ng_vlan_table_type=0A=
  	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP,=0A=
 +	  "getencap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_uint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP,=0A=
 +	  "setencap",=0A=
 +	  &ng_parse_uint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	  "getencapproto",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_uint16_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP_PROTO,=0A=
 +	  "setencapproto",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
  	{ 0 }=0A=
  };=0A=
  =0A=
 @@ -126,47 +178,43 @@=0A=
  };=0A=
  NETGRAPH_INIT(vlan, &ng_vlan_typestruct);=0A=
  =0A=
 -struct filter {=0A=
 -	LIST_ENTRY(filter) next;=0A=
 -	u_int16_t	vlan;=0A=
 -	hook_p		hook;=0A=
 -};=0A=
  =0A=
 -#define	HASHSIZE	16=0A=
 -#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)=0A=
 -LIST_HEAD(filterhead, filter);=0A=
  =0A=
 -typedef struct {=0A=
 -	hook_p		downstream_hook;=0A=
 -	hook_p		nomatch_hook;=0A=
 -	struct filterhead hashtable[HASHSIZE];=0A=
 -	u_int32_t	nent;=0A=
 -} *priv_p;=0A=
 +//**********************************************************************=
 **=0A=
 +//			HELPER STUFF=0A=
 +//**********************************************************************=
 **=0A=
  =0A=
 -static struct filter *=0A=
 -ng_vlan_findentry(priv_p priv, u_int16_t vlan)=0A=
 +static __inline int=0A=
 +m_chk(struct mbuf **mp, int len)=0A=
  {=0A=
 -	struct filterhead *chain =3D &priv->hashtable[HASH(vlan)];=0A=
 -	struct filter *f;=0A=
 +	if ((*mp)->m_pkthdr.len < len) {=0A=
 +		m_freem((*mp));=0A=
 +		(*mp) =3D NULL;=0A=
 +		return (EINVAL);=0A=
 +	}=0A=
 +	if ((*mp)->m_len < len && ((*mp) =3D m_pullup((*mp), len)) =3D=3D NULL)=0A=
 +		return (ENOBUFS);=0A=
  =0A=
 -	LIST_FOREACH(f, chain, next)=0A=
 -		if (f->vlan =3D=3D vlan)=0A=
 -			return (f);=0A=
 -	return (NULL);=0A=
 +return (0);=0A=
  }=0A=
  =0A=
 +//**********************************************************************=
 **=0A=
 +//			NETGRAPH NODE STUFF=0A=
 +//**********************************************************************=
 **=0A=
 +=0A=
  static int=0A=
  ng_vlan_constructor(node_p node)=0A=
  {=0A=
  	priv_p priv;=0A=
 -	int i;=0A=
  =0A=
  	priv =3D malloc(sizeof(*priv), M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
  	if (priv =3D=3D NULL)=0A=
  		return (ENOMEM);=0A=
 -	for (i =3D 0; i < HASHSIZE; i++)=0A=
 -		LIST_INIT(&priv->hashtable[i]);=0A=
 +	priv->encap_enable =3D 1;=0A=
 +	priv->encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 +	=0A=
  	NG_NODE_SET_PRIVATE(node, priv);=0A=
 +=0A=
  	return (0);=0A=
  }=0A=
  =0A=
 @@ -193,13 +241,14 @@=0A=
  ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(node);=0A=
 -	int error =3D 0;=0A=
  	struct ng_mesg *msg, *resp =3D NULL;=0A=
  	struct ng_vlan_filter *vf;=0A=
 -	struct filter *f;=0A=
  	hook_p hook;=0A=
  	struct ng_vlan_table *t;=0A=
 -	int i;=0A=
 +	uintptr_t hook_data;=0A=
 +	int i, vlan_count;=0A=
 +	u_int16_t vid;=0A=
 +	int error =3D 0;=0A=
  =0A=
  	NGI_GET_MSG(item, msg);=0A=
  	/* Deal with message according to cookie and command. */=0A=
 @@ -214,12 +263,12 @@=0A=
  			}=0A=
  			vf =3D (struct ng_vlan_filter *)msg->data;=0A=
  			/* Sanity check the VLAN ID value. */=0A=
 -			if (vf->vlan & ~EVL_VLID_MASK) {=0A=
 +			if (vf->vid & ~EVL_VLID_MASK || vf->pcp & ~7 || vf->cfi & ~1) {=0A=
  				error =3D EINVAL;=0A=
  				break;=0A=
  			}=0A=
  			/* Check that a referenced hook exists. */=0A=
 -			hook =3D ng_findhook(node, vf->hook);=0A=
 +			hook =3D ng_findhook(node, vf->hook_name);=0A=
  			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
 @@ -231,30 +280,18 @@=0A=
  				break;=0A=
  			}=0A=
  			/* And is not already in service. */=0A=
 -			if (NG_HOOK_PRIVATE(hook) !=3D NULL) {=0A=
 +			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
  			/* Check we don't already trap this VLAN. */=0A=
 -			if (ng_vlan_findentry(priv, vf->vlan)) {=0A=
 +			if (priv->vlan_hook[vf->vid] !=3D NULL) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
 -			/* Create filter. */=0A=
 -			f =3D malloc(sizeof(*f),=0A=
 -			    M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
 -			if (f =3D=3D NULL) {=0A=
 -				error =3D ENOMEM;=0A=
 -				break;=0A=
 -			}=0A=
 -			/* Link filter and hook together. */=0A=
 -			f->hook =3D hook;=0A=
 -			f->vlan =3D vf->vlan;=0A=
 -			NG_HOOK_SET_PRIVATE(hook, f);=0A=
 -			/* Register filter in a hash table. */=0A=
 -			LIST_INSERT_HEAD(=0A=
 -			    &priv->hashtable[HASH(f->vlan)], f, next);=0A=
 -			priv->nent++;=0A=
 +			/* Link vlan and hook together. */=0A=
 +			NG_HOOK_SET_PRIVATE(hook, (void *)(HOOK_VLAN_TAG_SET_MASK | =
 EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));=0A=
 +			priv->vlan_hook[vf->vid] =3D hook;=0A=
  			break;=0A=
  		case NGM_VLAN_DEL_FILTER:=0A=
  			/* Check that message is long enough. */=0A=
 @@ -264,35 +301,125 @@=0A=
  			}=0A=
  			/* Check that hook exists and is active. */=0A=
  			hook =3D ng_findhook(node, (char *)msg->data);=0A=
 -			if (hook =3D=3D NULL ||=0A=
 -			    (f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (priv->vlan_hook[EVL_VLANOFTAG(hook_data)] !=3D hook)=0A=
 +				printf("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook =3D %s\n", =
 __func__, (char *)msg->data);=0A=
 +#endif=0A=
 +			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
 +			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 +			break;=0A=
 +		case NGM_VLAN_DEL_VID_FLT:=0A=
 +			/* Check that message is long enough. */=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			vid =3D (*((u_int16_t *)msg->data));=0A=
 +			/* Sanity check the VLAN ID value. */=0A=
 +			if (vid & ~EVL_VLID_MASK) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			/* Check that hook exists and is active. */=0A=
 +			hook =3D priv->vlan_hook[vid];=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
  			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (EVL_VLANOFTAG(hook_data) !=3D vid)=0A=
 +				printf("%s: NGM_VLAN_DEL_VID_FLT: Invalid VID Hook =3D %us, must =
 be: %us\n", __func__, (u_int16_t)EVL_VLANOFTAG(hook_data), vid);=0A=
 +#endif=0A=
  			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[vid] =3D NULL;=0A=
  			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
  			break;=0A=
  		case NGM_VLAN_GET_TABLE:=0A=
 +			/* calculate vlans */=0A=
 +			vlan_count =3D 0;=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				if (priv->vlan_hook[i] !=3D NULL=0A=
 +				   && NG_HOOK_IS_VALID(priv->vlan_hook[i]))=0A=
 +					vlan_count ++;=0A=
 +			}=0A=
 +=0A=
 +			/* allocate memory for responce */=0A=
  			NG_MKRESPONSE(resp, msg, sizeof(*t) +=0A=
 -			    priv->nent * sizeof(*t->filter), M_NOWAIT);=0A=
 +			    vlan_count * sizeof(*t->filter), M_NOWAIT);=0A=
  			if (resp =3D=3D NULL) {=0A=
  				error =3D ENOMEM;=0A=
  				break;=0A=
  			}=0A=
 +=0A=
 +			/* pack data to responce */=0A=
  			t =3D (struct ng_vlan_table *)resp->data;=0A=
 -			t->n =3D priv->nent;=0A=
 +			t->n =3D 0;=0A=
  			vf =3D &t->filter[0];=0A=
 -			for (i =3D 0; i < HASHSIZE; i++) {=0A=
 -				LIST_FOREACH(f, &priv->hashtable[i], next) {=0A=
 -					vf->vlan =3D f->vlan;=0A=
 -					strncpy(vf->hook, NG_HOOK_NAME(f->hook),=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				hook =3D priv->vlan_hook[i];=0A=
 +				if (hook =3D=3D NULL=0A=
 +				   || NG_HOOK_NOT_VALID(hook))=0A=
 +					continue;=0A=
 +				hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +				if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)=0A=
 +					continue;=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +				if (EVL_VLANOFTAG(hook_data) !=3D i)=0A=
 +					printf("%s: NGM_VLAN_GET_TABLE: hook %s VID =3D %us, must be: =
 %i\n", __func__, NG_HOOK_NAME(hook), =
 (u_int16_t)EVL_VLANOFTAG(hook_data), i);=0A=
 +#endif=0A=
 +				vf->vid =3D i;=0A=
 +				vf->pcp =3D EVL_PRIOFTAG(hook_data);=0A=
 +				vf->cfi =3D EVL_CFIOFTAG(hook_data);=0A=
 +				strncpy(vf->hook_name, NG_HOOK_NAME(hook),=0A=
  					    NG_HOOKSIZ);=0A=
 -					vf++;=0A=
 -				}=0A=
 +				vf ++;=0A=
 +				t->n ++;=0A=
 +			}=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->encap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_enable =3D ((*((u_int32_t *)msg->data)) !=3D 0);=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP_PROTO:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int16_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int16_t *)resp->data)) =3D ntohs(priv->encap_proto);=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP_PROTO:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
  			}=0A=
 +			priv->encap_proto =3D htons((*((u_int16_t *)msg->data)));=0A=
  			break;=0A=
  		default:		/* Unknown command. */=0A=
  			error =3D EINVAL;=0A=
 @@ -302,8 +429,6 @@=0A=
  	case NGM_FLOW_COOKIE:=0A=
  	    {=0A=
  		struct ng_mesg *copy;=0A=
 -		struct filterhead *chain;=0A=
 -		struct filter *f;=0A=
  =0A=
  		/*=0A=
  		 * Flow control messages should come only=0A=
 @@ -314,17 +439,16 @@=0A=
  			break;=0A=
  		if (lasthook !=3D priv->downstream_hook)=0A=
  			break;=0A=
 -=0A=
  		/* Broadcast the event to all uplinks. */=0A=
 -		for (i =3D 0, chain =3D priv->hashtable; i < HASHSIZE;=0A=
 -		    i++, chain++)=0A=
 -		LIST_FOREACH(f, chain, next) {=0A=
 +		for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +			if (priv->vlan_hook[i] =3D=3D NULL)=0A=
 +				continue;=0A=
 +=0A=
  			NG_COPYMESSAGE(copy, msg, M_NOWAIT);=0A=
  			if (copy =3D=3D NULL)=0A=
 -				continue;=0A=
 -			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);=0A=
 +					continue;=0A=
 +			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);=0A=
  		}=0A=
 -=0A=
  		break;=0A=
  	    }=0A=
  	default:			/* Unknown type cookie. */=0A=
 @@ -343,16 +467,19 @@=0A=
  	struct ether_header *eh;=0A=
  	struct ether_vlan_header *evl =3D NULL;=0A=
  	int error;=0A=
 -	u_int16_t vlan;=0A=
 +	uintptr_t hook_data;=0A=
 +	u_int16_t vid;=0A=
  	struct mbuf *m;=0A=
 -	struct filter *f;=0A=
 +	hook_p dst_hook;=0A=
 +=0A=
  =0A=
 -	/* Make sure we have an entire header. */=0A=
  	NGI_GET_M(item, m);=0A=
 -	if (m->m_len < sizeof(*eh) &&=0A=
 -	    (m =3D m_pullup(m, sizeof(*eh))) =3D=3D NULL) {=0A=
 +=0A=
 +	/* Make sure we have an entire header. */=0A=
 +	error =3D m_chk(&m, ETHER_HDR_LEN);=0A=
 +	if (error !=3D 0) {=0A=
  		NG_FREE_ITEM(item);=0A=
 -		return (EINVAL);=0A=
 +		return (error);=0A=
  	}=0A=
  	eh =3D mtod(m, struct ether_header *);=0A=
  	if (hook =3D=3D priv->downstream_hook) {=0A=
 @@ -360,75 +487,104 @@=0A=
  		 * If from downstream, select between a match hook=0A=
  		 * or the nomatch hook.=0A=
  		 */=0A=
 +		dst_hook =3D priv->nomatch_hook;=0A=
  		if (m->m_flags & M_VLANTAG ||=0A=
 -		    eh->ether_type =3D=3D htons(ETHERTYPE_VLAN)) {=0A=
 +		    eh->ether_type =3D=3D priv->encap_proto) {=0A=
  			if (m->m_flags & M_VLANTAG) {=0A=
  				/*=0A=
  				 * Packet is tagged, m contains a normal=0A=
  				 * Ethernet frame; tag is stored out-of-band.=0A=
  				 */=0A=
 -				vlan =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 -			} else {=0A=
 -				if (m->m_len < sizeof(*evl) &&=0A=
 -				    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL) {=0A=
 +				vid =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 +			} else { /* eh->ether_type =3D=3D priv->encap_proto */=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0) {=0A=
  					NG_FREE_ITEM(item);=0A=
 -					return (EINVAL);=0A=
 +					return (error);=0A=
  				}=0A=
  				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -				vlan =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 +				vid =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
  			}=0A=
 -			if ((f =3D ng_vlan_findentry(priv, vlan)) !=3D NULL) {=0A=
 +=0A=
 +			if (priv->vlan_hook[vid] !=3D NULL) {=0A=
 +				dst_hook =3D priv->vlan_hook[vid];=0A=
  				if (m->m_flags & M_VLANTAG) {=0A=
  					m->m_pkthdr.ether_vtag =3D 0;=0A=
  					m->m_flags &=3D ~M_VLANTAG;=0A=
  				} else {=0A=
 -					evl->evl_encap_proto =3D evl->evl_proto;=0A=
 -					bcopy(mtod(m, caddr_t),=0A=
 -					    mtod(m, caddr_t) +=0A=
 -					    ETHER_VLAN_ENCAP_LEN,=0A=
 -					    ETHER_HDR_LEN);=0A=
 +					/* =0A=
 +					 * move DstMAC and SrcMAC to ETHER_TYPE=0A=
 +					 * before: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +					 *         |-----------------| =
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |---------------------|=0A=
 +					 * after: [free space] [dst_mac] [src_mac] [ether_type] [payload]=0A=
 +					 *                     |-----------------| |---------------------|=0A=
 +					 */=0A=
 +					bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +						(ETHER_ADDR_LEN * 2));=0A=
  					m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
  				}=0A=
  			}=0A=
 -		} else=0A=
 -			f =3D NULL;=0A=
 -		if (f !=3D NULL)=0A=
 -			NG_FWD_NEW_DATA(error, item, f->hook, m);=0A=
 -		else=0A=
 -			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);=0A=
 +		}=0A=
  	} else {=0A=
  		/*=0A=
  		 * It is heading towards the downstream.=0A=
  		 * If from nomatch, pass it unmodified.=0A=
  		 * Otherwise, do the VLAN encapsulation.=0A=
  		 */=0A=
 -		if (hook !=3D priv->nomatch_hook) {=0A=
 -			if ((f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +		dst_hook =3D priv->downstream_hook;=0A=
 +		if (dst_hook !=3D NULL && hook !=3D priv->nomatch_hook) {=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				m_freem(m);=0A=
  				NG_FREE_ITEM(item);=0A=
 -				NG_FREE_M(m);=0A=
  				return (EOPNOTSUPP);=0A=
  			}=0A=
 -			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 -			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 -			if (m =3D=3D NULL || (m->m_len < sizeof(*evl) &&=0A=
 -			    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL)) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				return (ENOMEM);=0A=
 +			if (priv->encap_enable =3D=3D 0) {=0A=
 +				/* just set packet header tag */=0A=
 +				m->m_flags |=3D M_VLANTAG;=0A=
 +				m->m_pkthdr.ether_vtag =3D (hook_data & VLAN_TAG_MASK);=0A=
 +			} else {=0A=
 +				/*=0A=
 +				 * Transform the Ethernet header into an Ethernet header=0A=
 +				 * with 802.1Q encapsulation.=0A=
 +				 * mod of: ether_vlanencap =0A=
 +				 */=0A=
 +				M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 +				/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 +				if (m =3D=3D NULL)=0A=
 +					error =3D ENOMEM;=0A=
 +				else=0A=
 +					error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0) {=0A=
 +					NG_FREE_ITEM(item);=0A=
 +					return (error);=0A=
 +				}=0A=
 +				/* move DstMAC and SrcMAC from ETHER_TYPE=0A=
 +				 * before: [free - prepended space] [dst_mac] [src_mac] =
 [ether_type] [payload]=0A=
 +				 *          <<<<<<<<<<<<<<<<<<<<<<  |-----------------| =
 |--------------------|=0A=
 +				 * after: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +				 *        |-----------------| |----------- inserted tag =
 -----------| |--------------------| =0A=
 +				 */=0A=
 +				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 +				bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +					(char *)evl, (ETHER_ADDR_LEN * 2));=0A=
 +				evl->evl_encap_proto =3D priv->encap_proto;=0A=
 +				evl->evl_tag =3D htons((hook_data & VLAN_TAG_MASK));=0A=
  			}=0A=
 -			/*=0A=
 -			 * Transform the Ethernet header into an Ethernet header=0A=
 -			 * with 802.1Q encapsulation.=0A=
 -			 */=0A=
 -			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,=0A=
 -			    mtod(m, char *), ETHER_HDR_LEN);=0A=
 -			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -			evl->evl_proto =3D evl->evl_encap_proto;=0A=
 -			evl->evl_encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 -			evl->evl_tag =3D htons(f->vlan);=0A=
  		}=0A=
 -		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);=0A=
  	}=0A=
 -	return (error);=0A=
 +=0A=
 +	/* send packet */=0A=
 +	if (dst_hook !=3D NULL) {=0A=
 +		NG_FWD_NEW_DATA(error, item, dst_hook, m);=0A=
 +		return (error);=0A=
 +	}=0A=
 +=0A=
 +	/* no hook to send */=0A=
 +	m_freem(m);=0A=
 +	NG_FREE_ITEM(item);=0A=
 +=0A=
 +	return (ENETDOWN);=0A=
  }=0A=
  =0A=
  static int=0A=
 @@ -446,7 +602,7 @@=0A=
  ng_vlan_disconnect(hook_p hook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
 -	struct filter *f;=0A=
 +	uintptr_t hook_data;=0A=
  =0A=
  	if (hook =3D=3D priv->downstream_hook)=0A=
  		priv->downstream_hook =3D NULL;=0A=
 @@ -454,11 +610,9 @@=0A=
  		priv->nomatch_hook =3D NULL;=0A=
  	else {=0A=
  		/* Purge a rule that refers to this hook. */=0A=
 -		if ((f =3D NG_HOOK_PRIVATE(hook)) !=3D NULL) {=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
 -		}=0A=
 +		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +		if (IS_HOOK_VLAN_SET(hook_data))=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
  	}=0A=
  	NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
  	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) =3D=3D 0) &&=0A=
 =0A=
 =0A=
 --- /usr/src/sys/netgraph/ng_vlan.h.orig	2009-08-03 17:13:06.000000000 =
 +0900=0A=
 +++ /usr/src/sys/netgraph/ng_vlan.h	2011-10-22 19:11:01.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -43,19 +44,28 @@=0A=
  enum {=0A=
  	NGM_VLAN_ADD_FILTER =3D 1,=0A=
  	NGM_VLAN_DEL_FILTER,=0A=
 -	NGM_VLAN_GET_TABLE=0A=
 +	NGM_VLAN_DEL_VID_FLT,=0A=
 +	NGM_VLAN_GET_TABLE,=0A=
 +	NGM_VLAN_GET_ENCAP,=0A=
 +	NGM_VLAN_SET_ENCAP,=0A=
 +	NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	NGM_VLAN_SET_ENCAP_PROTO,=0A=
  };=0A=
  =0A=
  /* For NGM_VLAN_ADD_FILTER control message. */=0A=
  struct ng_vlan_filter {=0A=
 -	char		hook[NG_HOOKSIZ];=0A=
 -	u_int16_t	vlan;=0A=
 -};	=0A=
 +	char		hook_name[NG_HOOKSIZ];=0A=
 +	u_int16_t	vid;	/* VID - VLAN Identifier */=0A=
 +	u_int8_t	pcp;	/* PCP - Priority Code Point */=0A=
 +	u_int8_t	cfi;	/* CFI - Canonical Format Indicator */=0A=
 +};=0A=
  =0A=
  /* Keep this in sync with the above structure definition.  */=0A=
  #define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 -	{ "hook",	&ng_parse_hookbuf_type  },		\=0A=
 -	{ "vlan",	&ng_parse_uint16_type   },		\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
  	{ NULL }						\=0A=
  }=0A=
  =0A=
 
 ------=_NextPart_000_033B_01CC935E.7927C480--
 
Responsible-Changed-From-To: freebsd-net->melifaro 
Responsible-Changed-By: melifaro 
Responsible-Changed-When: Sat Dec 3 17:23:08 UTC 2011 
Responsible-Changed-Why:  
Take 

http://www.freebsd.org/cgi/query-pr.cgi?pr=161908 

From: rozhuk.im@gmail.com
To: <bug-followup@FreeBSD.org>,
	<rozhuk.im@gmail.com>
Cc:  
Subject: Re: kern/161908: [netgraph] [patch] ng_vlan update for QinQ support
Date: Mon, 23 Jan 2012 17:28:01 +0900

 This is a multi-part message in MIME format.
 
 ------=_NextPart_000_0370_01CCD9F4.5C6E4580
 Content-Type: text/plain;
 	charset="us-ascii"
 Content-Transfer-Encoding: 7bit
 
 
 I took the opportunity and left the "vlan" in addfilter and gettable, while
 there you can enter and "vid".
 In addfilter if "vid" is not specified, it uses the "vlan".
 In gettable just returned both parameters, they are always equal.
 
 # ngctl msg re0:lower addfilter '{ vlan=1001 hook="vlan1001" }'
 equal
 # ngctl msg re0:lower addfilter '{ vid=1001 hook="vlan1001" }'
 
 # ngctl msg re0:lower gettable
 Rec'd response "gettable" (4) from "[14]:":
 Args:   { n=1 filter=[ { hook="vlan1001" vlan=1001 vid=1001 } ] }
 
 When it is not longer needed, simply remove
 # define NG_VLAN_USE_OLD_VLAN_NAME 1
 and rebuild a node
 
 
 I added one more option: decapsulate (getdecap NUM, setdecap NUM) - all
 packets coming from nomatch hook, the tag will be saved in the packet header
 (mbuf) and will be exposed flag M_VLANTAG. If the package has already been
 exposed to flag M_VLANTAG (when he entered the downstream hook), then
 decapsulation is performed.
 Default: 0 - disabled (old behavior).
 
 Encapsulate (getencap NUM, setencap NUM) - can take 4 values:
 0 - packets belonging to the filter hook is not encapsulated, the tag is
 written in the header mbuf flag and put up M_VLANTAG
 1 - The default (= VLAN_ENCAP_FROM_FILTER), packets are encapsulated hooks
 belonging filters (old behavior)
 2 - (= VLAN_ENCAP_FROM_NOMATCH) packages come with a nomatch hook flag
 M_VLANTAG - encapsulated, the flag is removed.
 3 = 1 + 2
 
 
 
 ------=_NextPart_000_0370_01CCD9F4.5C6E4580
 Content-Type: application/octet-stream;
 	name="ng_vlan.patch_v2"
 Content-Transfer-Encoding: quoted-printable
 Content-Disposition: attachment;
 	filename="ng_vlan.patch_v2"
 
 --- ./ng_vlan.c.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ./ng_vlan.c	2012-01-23 08:35:42.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -46,6 +47,23 @@=0A=
  #include <netgraph/ng_vlan.h>=0A=
  #include <netgraph/netgraph.h>=0A=
  =0A=
 +=0A=
 +=0A=
 +struct ng_vlan_private {=0A=
 +	hook_p		downstream_hook;=0A=
 +	hook_p		nomatch_hook;=0A=
 +	u_int32_t	decap_enable;=0A=
 +	u_int32_t	encap_enable;=0A=
 +	u_int16_t	encap_proto;=0A=
 +	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];=0A=
 +};=0A=
 +typedef struct ng_vlan_private *priv_p;=0A=
 +=0A=
 +#define VLAN_TAG_MASK	0xFFFF=0A=
 +#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))=0A=
 +#define IS_HOOK_VLAN_SET(hook_data) ((((uintptr_t)hook_data) & =
 HOOK_VLAN_TAG_SET_MASK) =3D=3D HOOK_VLAN_TAG_SET_MASK)=0A=
 +=0A=
 +=0A=
  static ng_constructor_t	ng_vlan_constructor;=0A=
  static ng_rcvmsg_t	ng_vlan_rcvmsg;=0A=
  static ng_shutdown_t	ng_vlan_shutdown;=0A=
 @@ -105,11 +123,60 @@=0A=
  	},=0A=
  	{=0A=
  	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_DEL_VID_FLT,=0A=
 +	  "delvidflt",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
  	  NGM_VLAN_GET_TABLE,=0A=
  	  "gettable",=0A=
  	  NULL,=0A=
  	  &ng_vlan_table_type=0A=
  	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_DECAP,=0A=
 +	  "getdecap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_uint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_DECAP,=0A=
 +	  "setdecap",=0A=
 +	  &ng_parse_uint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP,=0A=
 +	  "getencap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_uint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP,=0A=
 +	  "setencap",=0A=
 +	  &ng_parse_uint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	  "getencapproto",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_uint16_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP_PROTO,=0A=
 +	  "setencapproto",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
  	{ 0 }=0A=
  };=0A=
  =0A=
 @@ -126,44 +193,40 @@=0A=
  };=0A=
  NETGRAPH_INIT(vlan, &ng_vlan_typestruct);=0A=
  =0A=
 -struct filter {=0A=
 -	LIST_ENTRY(filter) next;=0A=
 -	u_int16_t	vlan;=0A=
 -	hook_p		hook;=0A=
 -};=0A=
  =0A=
 -#define	HASHSIZE	16=0A=
 -#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)=0A=
 -LIST_HEAD(filterhead, filter);=0A=
  =0A=
 -typedef struct {=0A=
 -	hook_p		downstream_hook;=0A=
 -	hook_p		nomatch_hook;=0A=
 -	struct filterhead hashtable[HASHSIZE];=0A=
 -	u_int32_t	nent;=0A=
 -} *priv_p;=0A=
 +//**********************************************************************=
 **=0A=
 +//			HELPER STUFF=0A=
 +//**********************************************************************=
 **=0A=
  =0A=
 -static struct filter *=0A=
 -ng_vlan_findentry(priv_p priv, u_int16_t vlan)=0A=
 +static __inline int=0A=
 +m_chk(struct mbuf **mp, int len)=0A=
  {=0A=
 -	struct filterhead *chain =3D &priv->hashtable[HASH(vlan)];=0A=
 -	struct filter *f;=0A=
 +	if ((*mp)->m_pkthdr.len < len) {=0A=
 +		m_freem((*mp));=0A=
 +		(*mp) =3D NULL;=0A=
 +		return (EINVAL);=0A=
 +	}=0A=
 +	if ((*mp)->m_len < len && ((*mp) =3D m_pullup((*mp), len)) =3D=3D NULL)=0A=
 +		return (ENOBUFS);=0A=
  =0A=
 -	LIST_FOREACH(f, chain, next)=0A=
 -		if (f->vlan =3D=3D vlan)=0A=
 -			return (f);=0A=
 -	return (NULL);=0A=
 +return (0);=0A=
  }=0A=
  =0A=
 +=0A=
 +//**********************************************************************=
 **=0A=
 +//			NETGRAPH NODE STUFF=0A=
 +//**********************************************************************=
 **=0A=
 +=0A=
  static int=0A=
  ng_vlan_constructor(node_p node)=0A=
  {=0A=
  	priv_p priv;=0A=
 -	int i;=0A=
  =0A=
  	priv =3D malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);=0A=
 -	for (i =3D 0; i < HASHSIZE; i++)=0A=
 -		LIST_INIT(&priv->hashtable[i]);=0A=
 +	priv->decap_enable =3D 0;=0A=
 +	priv->encap_enable =3D VLAN_ENCAP_FROM_FILTER;=0A=
 +	priv->encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
  	NG_NODE_SET_PRIVATE(node, priv);=0A=
  	return (0);=0A=
  }=0A=
 @@ -191,13 +254,14 @@=0A=
  ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(node);=0A=
 -	int error =3D 0;=0A=
  	struct ng_mesg *msg, *resp =3D NULL;=0A=
  	struct ng_vlan_filter *vf;=0A=
 -	struct filter *f;=0A=
  	hook_p hook;=0A=
  	struct ng_vlan_table *t;=0A=
 -	int i;=0A=
 +	uintptr_t hook_data;=0A=
 +	int i, vlan_count;=0A=
 +	u_int16_t vid;=0A=
 +	int error =3D 0;=0A=
  =0A=
  	NGI_GET_MSG(item, msg);=0A=
  	/* Deal with message according to cookie and command. */=0A=
 @@ -212,12 +276,20 @@=0A=
  			}=0A=
  			vf =3D (struct ng_vlan_filter *)msg->data;=0A=
  			/* Sanity check the VLAN ID value. */=0A=
 -			if (vf->vlan & ~EVL_VLID_MASK) {=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +			if (vf->vid =3D=3D 0 && vf->vid !=3D vf->vlan)=0A=
 +				vf->vid =3D vf->vlan;=0A=
 +			else if (vf->vid !=3D 0 && vf->vlan !=3D0 && vf->vid !=3D vf->vlan) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +#endif=0A=
 +			if (vf->vid & ~EVL_VLID_MASK || vf->pcp & ~7 || vf->cfi & ~1) {=0A=
  				error =3D EINVAL;=0A=
  				break;=0A=
  			}=0A=
  			/* Check that a referenced hook exists. */=0A=
 -			hook =3D ng_findhook(node, vf->hook);=0A=
 +			hook =3D ng_findhook(node, vf->hook_name);=0A=
  			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
 @@ -229,30 +301,18 @@=0A=
  				break;=0A=
  			}=0A=
  			/* And is not already in service. */=0A=
 -			if (NG_HOOK_PRIVATE(hook) !=3D NULL) {=0A=
 +			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
  			/* Check we don't already trap this VLAN. */=0A=
 -			if (ng_vlan_findentry(priv, vf->vlan)) {=0A=
 +			if (priv->vlan_hook[vf->vid] !=3D NULL) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
 -			/* Create filter. */=0A=
 -			f =3D malloc(sizeof(*f),=0A=
 -			    M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
 -			if (f =3D=3D NULL) {=0A=
 -				error =3D ENOMEM;=0A=
 -				break;=0A=
 -			}=0A=
 -			/* Link filter and hook together. */=0A=
 -			f->hook =3D hook;=0A=
 -			f->vlan =3D vf->vlan;=0A=
 -			NG_HOOK_SET_PRIVATE(hook, f);=0A=
 -			/* Register filter in a hash table. */=0A=
 -			LIST_INSERT_HEAD(=0A=
 -			    &priv->hashtable[HASH(f->vlan)], f, next);=0A=
 -			priv->nent++;=0A=
 +			/* Link vlan and hook together. */=0A=
 +			NG_HOOK_SET_PRIVATE(hook, (void *)(HOOK_VLAN_TAG_SET_MASK | =
 EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));=0A=
 +			priv->vlan_hook[vf->vid] =3D hook;=0A=
  			break;=0A=
  		case NGM_VLAN_DEL_FILTER:=0A=
  			/* Check that message is long enough. */=0A=
 @@ -262,35 +322,143 @@=0A=
  			}=0A=
  			/* Check that hook exists and is active. */=0A=
  			hook =3D ng_findhook(node, (char *)msg->data);=0A=
 -			if (hook =3D=3D NULL ||=0A=
 -			    (f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
  			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (priv->vlan_hook[EVL_VLANOFTAG(hook_data)] !=3D hook)=0A=
 +				printf("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook =3D %s\n", =
 __func__, (char *)msg->data);=0A=
 +#endif=0A=
  			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
 +			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 +			break;=0A=
 +		case NGM_VLAN_DEL_VID_FLT:=0A=
 +			/* Check that message is long enough. */=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			vid =3D (*((u_int16_t *)msg->data));=0A=
 +			/* Sanity check the VLAN ID value. */=0A=
 +			if (vid & ~EVL_VLID_MASK) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			/* Check that hook exists and is active. */=0A=
 +			hook =3D priv->vlan_hook[vid];=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (EVL_VLANOFTAG(hook_data) !=3D vid)=0A=
 +				printf("%s: NGM_VLAN_DEL_VID_FLT: Invalid VID Hook =3D %us, must =
 be: %us\n", __func__, (u_int16_t)EVL_VLANOFTAG(hook_data), vid);=0A=
 +#endif=0A=
 +			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[vid] =3D NULL;=0A=
  			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
  			break;=0A=
  		case NGM_VLAN_GET_TABLE:=0A=
 +			/* calculate vlans */=0A=
 +			vlan_count =3D 0;=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				if (priv->vlan_hook[i] !=3D NULL=0A=
 +				   && NG_HOOK_IS_VALID(priv->vlan_hook[i]))=0A=
 +					vlan_count ++;=0A=
 +			}=0A=
 +=0A=
 +			/* allocate memory for responce */=0A=
  			NG_MKRESPONSE(resp, msg, sizeof(*t) +=0A=
 -			    priv->nent * sizeof(*t->filter), M_NOWAIT);=0A=
 +			    vlan_count * sizeof(*t->filter), M_NOWAIT);=0A=
  			if (resp =3D=3D NULL) {=0A=
  				error =3D ENOMEM;=0A=
  				break;=0A=
  			}=0A=
 +=0A=
 +			/* pack data to responce */=0A=
  			t =3D (struct ng_vlan_table *)resp->data;=0A=
 -			t->n =3D priv->nent;=0A=
 +			t->n =3D 0;=0A=
  			vf =3D &t->filter[0];=0A=
 -			for (i =3D 0; i < HASHSIZE; i++) {=0A=
 -				LIST_FOREACH(f, &priv->hashtable[i], next) {=0A=
 -					vf->vlan =3D f->vlan;=0A=
 -					strncpy(vf->hook, NG_HOOK_NAME(f->hook),=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				hook =3D priv->vlan_hook[i];=0A=
 +				if (hook =3D=3D NULL=0A=
 +				   || NG_HOOK_NOT_VALID(hook))=0A=
 +					continue;=0A=
 +				hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +				if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)=0A=
 +					continue;=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +				if (EVL_VLANOFTAG(hook_data) !=3D i)=0A=
 +					printf("%s: NGM_VLAN_GET_TABLE: hook %s VID =3D %us, must be: =
 %i\n", __func__, NG_HOOK_NAME(hook), =
 (u_int16_t)EVL_VLANOFTAG(hook_data), i);=0A=
 +#endif=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +				vf->vlan =3D i;=0A=
 +#endif=0A=
 +				vf->vid =3D i;=0A=
 +				vf->pcp =3D EVL_PRIOFTAG(hook_data);=0A=
 +				vf->cfi =3D EVL_CFIOFTAG(hook_data);=0A=
 +				strncpy(vf->hook_name, NG_HOOK_NAME(hook),=0A=
  					    NG_HOOKSIZ);=0A=
 -					vf++;=0A=
 -				}=0A=
 +				vf ++;=0A=
 +				t->n ++;=0A=
 +			}=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_DECAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->decap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_DECAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->decap_enable =3D (*((u_int32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->encap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_enable =3D (*((u_int32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP_PROTO:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int16_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
  			}=0A=
 +			(*((u_int16_t *)resp->data)) =3D ntohs(priv->encap_proto);=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP_PROTO:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_proto =3D htons((*((u_int16_t *)msg->data)));=0A=
  			break;=0A=
  		default:		/* Unknown command. */=0A=
  			error =3D EINVAL;=0A=
 @@ -300,8 +468,6 @@=0A=
  	case NGM_FLOW_COOKIE:=0A=
  	    {=0A=
  		struct ng_mesg *copy;=0A=
 -		struct filterhead *chain;=0A=
 -		struct filter *f;=0A=
  =0A=
  		/*=0A=
  		 * Flow control messages should come only=0A=
 @@ -312,17 +478,16 @@=0A=
  			break;=0A=
  		if (lasthook !=3D priv->downstream_hook)=0A=
  			break;=0A=
 -=0A=
  		/* Broadcast the event to all uplinks. */=0A=
 -		for (i =3D 0, chain =3D priv->hashtable; i < HASHSIZE;=0A=
 -		    i++, chain++)=0A=
 -		LIST_FOREACH(f, chain, next) {=0A=
 +		for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +			if (priv->vlan_hook[i] =3D=3D NULL)=0A=
 +				continue;=0A=
 +=0A=
  			NG_COPYMESSAGE(copy, msg, M_NOWAIT);=0A=
  			if (copy =3D=3D NULL)=0A=
 -				continue;=0A=
 -			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);=0A=
 +					continue;=0A=
 +			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);=0A=
  		}=0A=
 -=0A=
  		break;=0A=
  	    }=0A=
  	default:			/* Unknown type cookie. */=0A=
 @@ -340,92 +505,155 @@=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
  	struct ether_header *eh;=0A=
  	struct ether_vlan_header *evl =3D NULL;=0A=
 -	int error;=0A=
 -	u_int16_t vlan;=0A=
 +	int error, encapsulate =3D 0;=0A=
 +	uintptr_t hook_data;=0A=
 +	u_int16_t vid, eth_vtag;=0A=
  	struct mbuf *m;=0A=
 -	struct filter *f;=0A=
 +	hook_p dst_hook;=0A=
 +=0A=
  =0A=
 -	/* Make sure we have an entire header. */=0A=
  	NGI_GET_M(item, m);=0A=
 -	if (m->m_len < sizeof(*eh) &&=0A=
 -	    (m =3D m_pullup(m, sizeof(*eh))) =3D=3D NULL) {=0A=
 -		NG_FREE_ITEM(item);=0A=
 -		return (EINVAL);=0A=
 -	}=0A=
 +=0A=
 +	/* Make sure we have an entire header. */=0A=
 +	error =3D m_chk(&m, ETHER_HDR_LEN);=0A=
 +	if (error !=3D 0)=0A=
 +		goto mchk_err;=0A=
 +=0A=
  	eh =3D mtod(m, struct ether_header *);=0A=
  	if (hook =3D=3D priv->downstream_hook) {=0A=
  		/*=0A=
  		 * If from downstream, select between a match hook=0A=
  		 * or the nomatch hook.=0A=
  		 */=0A=
 -		if (m->m_flags & M_VLANTAG ||=0A=
 -		    eh->ether_type =3D=3D htons(ETHERTYPE_VLAN)) {=0A=
 +		dst_hook =3D priv->nomatch_hook;=0A=
 +		if (m->m_flags & M_VLANTAG=0A=
 +		    || eh->ether_type =3D=3D priv->encap_proto) {=0A=
  			if (m->m_flags & M_VLANTAG) {=0A=
  				/*=0A=
  				 * Packet is tagged, m contains a normal=0A=
  				 * Ethernet frame; tag is stored out-of-band.=0A=
  				 */=0A=
 -				vlan =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 -			} else {=0A=
 -				if (m->m_len < sizeof(*evl) &&=0A=
 -				    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL) {=0A=
 -					NG_FREE_ITEM(item);=0A=
 -					return (EINVAL);=0A=
 -				}=0A=
 +				/* evl =3D NULL; */=0A=
 +				vid =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 +			} else { /* eh->ether_type =3D=3D priv->encap_proto */=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0)=0A=
 +					goto mchk_err;=0A=
 +=0A=
  				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -				vlan =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 +				vid =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
  			}=0A=
 -			if ((f =3D ng_vlan_findentry(priv, vlan)) !=3D NULL) {=0A=
 -				if (m->m_flags & M_VLANTAG) {=0A=
 -					m->m_pkthdr.ether_vtag =3D 0;=0A=
 -					m->m_flags &=3D ~M_VLANTAG;=0A=
 +=0A=
 +			if (priv->vlan_hook[vid] !=3D NULL) {=0A=
 +				/* vlan filter: allways remove all vlan tags and decapsulate packet =
 */=0A=
 +				dst_hook =3D priv->vlan_hook[vid];=0A=
 +				m->m_pkthdr.ether_vtag =3D 0;=0A=
 +				m->m_flags &=3D ~M_VLANTAG;=0A=
 +			} else if (evl !=3D NULL) { /* nomatch_hook and encapsulated packet =
 */=0A=
 +				if (priv->decap_enable =3D=3D 0) {=0A=
 +					evl =3D NULL;=0A=
  				} else {=0A=
 -					evl->evl_encap_proto =3D evl->evl_proto;=0A=
 -					bcopy(mtod(m, caddr_t),=0A=
 -					    mtod(m, caddr_t) +=0A=
 -					    ETHER_VLAN_ENCAP_LEN,=0A=
 -					    ETHER_HDR_LEN);=0A=
 -					m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
 +					m->m_pkthdr.ether_vtag =3D ntohs(evl->evl_tag);=0A=
 +					m->m_flags |=3D M_VLANTAG;=0A=
  				}=0A=
  			}=0A=
 -		} else=0A=
 -			f =3D NULL;=0A=
 -		if (f !=3D NULL)=0A=
 -			NG_FWD_NEW_DATA(error, item, f->hook, m);=0A=
 -		else=0A=
 -			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);=0A=
 +=0A=
 +			if (evl !=3D NULL) {=0A=
 +				/* =0A=
 +				 * decapsulate=0A=
 +				 * move DstMAC and SrcMAC to ETHER_TYPE=0A=
 +				 * before: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +				 *         |-----------------| =
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |---------------------|=0A=
 +				 * after: [free space] [dst_mac] [src_mac] [ether_type] [payload]=0A=
 +				 *                     |-----------------| |---------------------|=0A=
 +				 */=0A=
 +				bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +					(ETHER_ADDR_LEN * 2));=0A=
 +				m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
 +			}=0A=
 +		}=0A=
 +=0A=
 +		if (dst_hook =3D=3D NULL) {=0A=
 +			error =3D ENETDOWN;=0A=
 +			goto drop;=0A=
 +		}=0A=
  	} else {=0A=
  		/*=0A=
  		 * It is heading towards the downstream.=0A=
  		 * If from nomatch, pass it unmodified.=0A=
  		 * Otherwise, do the VLAN encapsulation.=0A=
  		 */=0A=
 -		if (hook !=3D priv->nomatch_hook) {=0A=
 -			if ((f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				NG_FREE_M(m);=0A=
 -				return (EOPNOTSUPP);=0A=
 +		dst_hook =3D priv->downstream_hook;=0A=
 +		if (dst_hook =3D=3D NULL) {=0A=
 +			error =3D ENETDOWN;=0A=
 +			goto drop;=0A=
 +		}=0A=
 +		if (hook !=3D priv->nomatch_hook) {/* filter hook */=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				/*=0A=
 +				 * packet from hook not in filter=0A=
 +				 * call addfilter for this hook to fix=0A=
 +				 */=0A=
 +				error =3D EOPNOTSUPP;=0A=
 +				goto drop;=0A=
  			}=0A=
 -			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 -			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 -			if (m =3D=3D NULL || (m->m_len < sizeof(*evl) &&=0A=
 -			    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL)) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				return (ENOMEM);=0A=
 +			eth_vtag =3D (hook_data & VLAN_TAG_MASK);=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) !=3D 0) {=0A=
 +				encapsulate =3D 1;=0A=
 +			} else {=0A=
 +				/* just set packet header tag */=0A=
 +				m->m_flags |=3D M_VLANTAG;=0A=
 +				m->m_pkthdr.ether_vtag =3D eth_vtag;=0A=
 +			}=0A=
 +		} else { /* nomatch_hook */=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) !=3D 0=0A=
 +			    && m->m_flags & M_VLANTAG) {=0A=
 +				/* encapsulate tagged packet */=0A=
 +				eth_vtag =3D m->m_pkthdr.ether_vtag;=0A=
 +				m->m_pkthdr.ether_vtag =3D 0;=0A=
 +				m->m_flags &=3D ~M_VLANTAG;=0A=
 +				encapsulate =3D 1;=0A=
  			}=0A=
 +		}=0A=
 +=0A=
 +		if (encapsulate !=3D 0) {=0A=
  			/*=0A=
  			 * Transform the Ethernet header into an Ethernet header=0A=
  			 * with 802.1Q encapsulation.=0A=
 +			 * mod of: ether_vlanencap =0A=
 +			 */=0A=
 +			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 +			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 +			if (m =3D=3D NULL)=0A=
 +				error =3D ENOMEM;=0A=
 +			else=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +			if (error !=3D 0)=0A=
 +				goto mchk_err;=0A=
 +=0A=
 +			/* move DstMAC and SrcMAC from ETHER_TYPE=0A=
 +			 * before: [free - prepended space] [dst_mac] [src_mac] [ether_type] =
 [payload]=0A=
 +			 *          <<<<<<<<<<<<<<<<<<<<<<  |-----------------| =
 |--------------------|=0A=
 +			 * after: [dst_mac] [src_mac] [ether_type_encap(TPID)] [PCP/CFI/VID] =
 [ether_type] [payload]=0A=
 +			 *        |-----------------| |----------- inserted tag -----------| =
 |--------------------| =0A=
  			 */=0A=
 -			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,=0A=
 -			    mtod(m, char *), ETHER_HDR_LEN);=0A=
  			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -			evl->evl_proto =3D evl->evl_encap_proto;=0A=
 -			evl->evl_encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 -			evl->evl_tag =3D htons(f->vlan);=0A=
 +			bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +				(char *)evl, (ETHER_ADDR_LEN * 2));=0A=
 +			evl->evl_encap_proto =3D priv->encap_proto;=0A=
 +			evl->evl_tag =3D htons(eth_vtag);=0A=
  		}=0A=
 -		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);=0A=
  	}=0A=
 +=0A=
 +	/* send packet */=0A=
 +	NG_FWD_NEW_DATA(error, item, dst_hook, m);=0A=
 +	return (error);=0A=
 +=0A=
 +drop:=0A=
 +	m_freem(m);=0A=
 +mchk_err:=0A=
 +	NG_FREE_ITEM(item);=0A=
  	return (error);=0A=
  }=0A=
  =0A=
 @@ -444,7 +672,7 @@=0A=
  ng_vlan_disconnect(hook_p hook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
 -	struct filter *f;=0A=
 +	uintptr_t hook_data;=0A=
  =0A=
  	if (hook =3D=3D priv->downstream_hook)=0A=
  		priv->downstream_hook =3D NULL;=0A=
 @@ -452,11 +680,9 @@=0A=
  		priv->nomatch_hook =3D NULL;=0A=
  	else {=0A=
  		/* Purge a rule that refers to this hook. */=0A=
 -		if ((f =3D NG_HOOK_PRIVATE(hook)) !=3D NULL) {=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
 -		}=0A=
 +		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +		if (IS_HOOK_VLAN_SET(hook_data))=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
  	}=0A=
  	NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
  	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) =3D=3D 0) &&=0A=
 --- ./ng_vlan.h.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ./ng_vlan.h	2012-01-23 07:34:26.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -31,6 +32,10 @@=0A=
  #ifndef _NETGRAPH_NG_VLAN_H_=0A=
  #define	_NETGRAPH_NG_VLAN_H_=0A=
  =0A=
 +#define NG_VLAN_USE_OLD_VLAN_NAME 1 /* using "vlan" in addfilter and =
 gettable messages. 2012.01 */=0A=
 +=0A=
 +=0A=
 +=0A=
  /* Node type name and magic cookie. */=0A=
  #define	NG_VLAN_NODE_TYPE	"vlan"=0A=
  #define	NGM_VLAN_COOKIE		1068486472=0A=
 @@ -43,21 +48,51 @@=0A=
  enum {=0A=
  	NGM_VLAN_ADD_FILTER =3D 1,=0A=
  	NGM_VLAN_DEL_FILTER,=0A=
 -	NGM_VLAN_GET_TABLE=0A=
 +	NGM_VLAN_DEL_VID_FLT,=0A=
 +	NGM_VLAN_GET_TABLE,=0A=
 +	NGM_VLAN_GET_DECAP,=0A=
 +	NGM_VLAN_SET_DECAP,=0A=
 +	NGM_VLAN_GET_ENCAP,=0A=
 +	NGM_VLAN_SET_ENCAP,=0A=
 +	NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	NGM_VLAN_SET_ENCAP_PROTO,=0A=
  };=0A=
  =0A=
 +#define VLAN_ENCAP_FROM_FILTER	0x00000001=0A=
 +#define VLAN_ENCAP_FROM_NOMATCH	0x00000002=0A=
 +=0A=
 +=0A=
  /* For NGM_VLAN_ADD_FILTER control message. */=0A=
  struct ng_vlan_filter {=0A=
 -	char		hook[NG_HOOKSIZ];=0A=
 -	u_int16_t	vlan;=0A=
 -};	=0A=
 +	char		hook_name[NG_HOOKSIZ];=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +	u_int16_t	vlan;	/* VLAN -  VLAN Identifier - same as vid, oldname, =
 deprecated */=0A=
 +#endif=0A=
 +	u_int16_t	vid;	/* VID - VLAN Identifier */=0A=
 +	u_int8_t	pcp;	/* PCP - Priority Code Point */=0A=
 +	u_int8_t	cfi;	/* CFI - Canonical Format Indicator */=0A=
 +};=0A=
  =0A=
  /* Keep this in sync with the above structure definition.  */=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
  #define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 -	{ "hook",	&ng_parse_hookbuf_type  },		\=0A=
 -	{ "vlan",	&ng_parse_uint16_type   },		\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vlan",	&ng_parse_uint16_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
  	{ NULL }						\=0A=
  }=0A=
 +#else=0A=
 +#define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
 +	{ NULL }						\=0A=
 +}=0A=
 +#endif=0A=
 +=0A=
  =0A=
  /* Structure returned by NGM_VLAN_GET_TABLE. */=0A=
  struct ng_vlan_table {=0A=
 
 ------=_NextPart_000_0370_01CCD9F4.5C6E4580--
 

From: rozhuk.im@gmail.com
To: <bug-followup@FreeBSD.org>
Cc:  
Subject: Re: kern/161908: [netgraph] [patch] ng_vlan update for QinQ support
Date: Wed, 25 Jan 2012 00:57:09 +0900

 This is a multi-part message in MIME format.
 
 ------=_NextPart_000_0017_01CCDAFC.44F8C6B0
 Content-Type: text/plain;
 	charset="us-ascii"
 Content-Transfer-Encoding: 7bit
 
 
 Changed ng_parce_types in struct ng_cmdlist ng_vlan_cmdlist:
 
 "getdecap": ng_parse_uint32_type -> ng_parse_hint32_type
 "setdecap": ng_parse_uint32_type -> ng_parse_hint32_type
 "getencap": ng_parse_uint32_type -> ng_parse_hint32_type
 "setencap": ng_parse_uint32_type -> ng_parse_hint32_type
 "getencapproto": ng_parse_uint16_type -> ng_parse_hint16_type
 "setencapproto": ng_parse_uint16_type -> ng_parse_hint16_type
 
 l# ngctl msg re0:lower getencapproto
 Rec'd response "getencapproto" (9) from "[15ac]:":
 Args:   0x8100
 
 # ngctl msg re0:lower setencapproto 8000
 
 # ngctl msg re0:lower getencapproto
 Rec'd response "getencapproto" (9) from "[15ac]:":
 Args:   0x1f40
 
 # ngctl msg re0:lower setencapproto 0x8000 
 root@firewall# ngctl msg re0:lower getencapproto
 Rec'd response "getencapproto" (9) from "[15ac]:":
 Args:   0x8000
 
 
 # ngctl msg re0:lower getencap
 Rec'd response "getencap" (7) from "[15b8]:":
 Args:   0x1
 
 # ngctl msg re0:lower getdecap
 Rec'd response "getdecap" (5) from "[15b8]:":
 Args:   0x0
 
 
 
 ------=_NextPart_000_0017_01CCDAFC.44F8C6B0
 Content-Type: application/octet-stream;
 	name="ng_vlan.patch_v3"
 Content-Transfer-Encoding: quoted-printable
 Content-Disposition: attachment;
 	filename="ng_vlan.patch_v3"
 
 --- ./ng_vlan.c.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ./ng_vlan.c	2012-01-24 22:53:20.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -46,6 +47,23 @@=0A=
  #include <netgraph/ng_vlan.h>=0A=
  #include <netgraph/netgraph.h>=0A=
  =0A=
 +=0A=
 +=0A=
 +struct ng_vlan_private {=0A=
 +	hook_p		downstream_hook;=0A=
 +	hook_p		nomatch_hook;=0A=
 +	u_int32_t	decap_enable;=0A=
 +	u_int32_t	encap_enable;=0A=
 +	u_int16_t	encap_proto;=0A=
 +	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];=0A=
 +};=0A=
 +typedef struct ng_vlan_private *priv_p;=0A=
 +=0A=
 +#define VLAN_TAG_MASK	0xFFFF=0A=
 +#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))=0A=
 +#define IS_HOOK_VLAN_SET(hook_data) ((((uintptr_t)hook_data) & =
 HOOK_VLAN_TAG_SET_MASK) =3D=3D HOOK_VLAN_TAG_SET_MASK)=0A=
 +=0A=
 +=0A=
  static ng_constructor_t	ng_vlan_constructor;=0A=
  static ng_rcvmsg_t	ng_vlan_rcvmsg;=0A=
  static ng_shutdown_t	ng_vlan_shutdown;=0A=
 @@ -105,11 +123,60 @@=0A=
  	},=0A=
  	{=0A=
  	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_DEL_VID_FLT,=0A=
 +	  "delvidflt",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
  	  NGM_VLAN_GET_TABLE,=0A=
  	  "gettable",=0A=
  	  NULL,=0A=
  	  &ng_vlan_table_type=0A=
  	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_DECAP,=0A=
 +	  "getdecap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_DECAP,=0A=
 +	  "setdecap",=0A=
 +	  &ng_parse_hint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP,=0A=
 +	  "getencap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP,=0A=
 +	  "setencap",=0A=
 +	  &ng_parse_hint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	  "getencapproto",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint16_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP_PROTO,=0A=
 +	  "setencapproto",=0A=
 +	  &ng_parse_hint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
  	{ 0 }=0A=
  };=0A=
  =0A=
 @@ -126,44 +193,40 @@=0A=
  };=0A=
  NETGRAPH_INIT(vlan, &ng_vlan_typestruct);=0A=
  =0A=
 -struct filter {=0A=
 -	LIST_ENTRY(filter) next;=0A=
 -	u_int16_t	vlan;=0A=
 -	hook_p		hook;=0A=
 -};=0A=
  =0A=
 -#define	HASHSIZE	16=0A=
 -#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)=0A=
 -LIST_HEAD(filterhead, filter);=0A=
  =0A=
 -typedef struct {=0A=
 -	hook_p		downstream_hook;=0A=
 -	hook_p		nomatch_hook;=0A=
 -	struct filterhead hashtable[HASHSIZE];=0A=
 -	u_int32_t	nent;=0A=
 -} *priv_p;=0A=
 +//**********************************************************************=
 **=0A=
 +//			HELPER STUFF=0A=
 +//**********************************************************************=
 **=0A=
  =0A=
 -static struct filter *=0A=
 -ng_vlan_findentry(priv_p priv, u_int16_t vlan)=0A=
 +static __inline int=0A=
 +m_chk(struct mbuf **mp, int len)=0A=
  {=0A=
 -	struct filterhead *chain =3D &priv->hashtable[HASH(vlan)];=0A=
 -	struct filter *f;=0A=
 +	if ((*mp)->m_pkthdr.len < len) {=0A=
 +		m_freem((*mp));=0A=
 +		(*mp) =3D NULL;=0A=
 +		return (EINVAL);=0A=
 +	}=0A=
 +	if ((*mp)->m_len < len && ((*mp) =3D m_pullup((*mp), len)) =3D=3D NULL)=0A=
 +		return (ENOBUFS);=0A=
  =0A=
 -	LIST_FOREACH(f, chain, next)=0A=
 -		if (f->vlan =3D=3D vlan)=0A=
 -			return (f);=0A=
 -	return (NULL);=0A=
 +return (0);=0A=
  }=0A=
  =0A=
 +=0A=
 +//**********************************************************************=
 **=0A=
 +//			NETGRAPH NODE STUFF=0A=
 +//**********************************************************************=
 **=0A=
 +=0A=
  static int=0A=
  ng_vlan_constructor(node_p node)=0A=
  {=0A=
  	priv_p priv;=0A=
 -	int i;=0A=
  =0A=
  	priv =3D malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);=0A=
 -	for (i =3D 0; i < HASHSIZE; i++)=0A=
 -		LIST_INIT(&priv->hashtable[i]);=0A=
 +	priv->decap_enable =3D 0;=0A=
 +	priv->encap_enable =3D VLAN_ENCAP_FROM_FILTER;=0A=
 +	priv->encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
  	NG_NODE_SET_PRIVATE(node, priv);=0A=
  	return (0);=0A=
  }=0A=
 @@ -191,13 +254,14 @@=0A=
  ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(node);=0A=
 -	int error =3D 0;=0A=
  	struct ng_mesg *msg, *resp =3D NULL;=0A=
  	struct ng_vlan_filter *vf;=0A=
 -	struct filter *f;=0A=
  	hook_p hook;=0A=
  	struct ng_vlan_table *t;=0A=
 -	int i;=0A=
 +	uintptr_t hook_data;=0A=
 +	int i, vlan_count;=0A=
 +	u_int16_t vid;=0A=
 +	int error =3D 0;=0A=
  =0A=
  	NGI_GET_MSG(item, msg);=0A=
  	/* Deal with message according to cookie and command. */=0A=
 @@ -212,12 +276,20 @@=0A=
  			}=0A=
  			vf =3D (struct ng_vlan_filter *)msg->data;=0A=
  			/* Sanity check the VLAN ID value. */=0A=
 -			if (vf->vlan & ~EVL_VLID_MASK) {=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +			if (vf->vid =3D=3D 0 && vf->vid !=3D vf->vlan)=0A=
 +				vf->vid =3D vf->vlan;=0A=
 +			else if (vf->vid !=3D 0 && vf->vlan !=3D0 && vf->vid !=3D vf->vlan) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +#endif=0A=
 +			if (vf->vid & ~EVL_VLID_MASK || vf->pcp & ~7 || vf->cfi & ~1) {=0A=
  				error =3D EINVAL;=0A=
  				break;=0A=
  			}=0A=
  			/* Check that a referenced hook exists. */=0A=
 -			hook =3D ng_findhook(node, vf->hook);=0A=
 +			hook =3D ng_findhook(node, vf->hook_name);=0A=
  			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
 @@ -229,30 +301,18 @@=0A=
  				break;=0A=
  			}=0A=
  			/* And is not already in service. */=0A=
 -			if (NG_HOOK_PRIVATE(hook) !=3D NULL) {=0A=
 +			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
  			/* Check we don't already trap this VLAN. */=0A=
 -			if (ng_vlan_findentry(priv, vf->vlan)) {=0A=
 +			if (priv->vlan_hook[vf->vid] !=3D NULL) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
 -			/* Create filter. */=0A=
 -			f =3D malloc(sizeof(*f),=0A=
 -			    M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
 -			if (f =3D=3D NULL) {=0A=
 -				error =3D ENOMEM;=0A=
 -				break;=0A=
 -			}=0A=
 -			/* Link filter and hook together. */=0A=
 -			f->hook =3D hook;=0A=
 -			f->vlan =3D vf->vlan;=0A=
 -			NG_HOOK_SET_PRIVATE(hook, f);=0A=
 -			/* Register filter in a hash table. */=0A=
 -			LIST_INSERT_HEAD(=0A=
 -			    &priv->hashtable[HASH(f->vlan)], f, next);=0A=
 -			priv->nent++;=0A=
 +			/* Link vlan and hook together. */=0A=
 +			NG_HOOK_SET_PRIVATE(hook, (void *)(HOOK_VLAN_TAG_SET_MASK | =
 EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));=0A=
 +			priv->vlan_hook[vf->vid] =3D hook;=0A=
  			break;=0A=
  		case NGM_VLAN_DEL_FILTER:=0A=
  			/* Check that message is long enough. */=0A=
 @@ -262,35 +322,143 @@=0A=
  			}=0A=
  			/* Check that hook exists and is active. */=0A=
  			hook =3D ng_findhook(node, (char *)msg->data);=0A=
 -			if (hook =3D=3D NULL ||=0A=
 -			    (f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
  			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (priv->vlan_hook[EVL_VLANOFTAG(hook_data)] !=3D hook)=0A=
 +				printf("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook =3D %s\n", =
 __func__, (char *)msg->data);=0A=
 +#endif=0A=
  			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
 +			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 +			break;=0A=
 +		case NGM_VLAN_DEL_VID_FLT:=0A=
 +			/* Check that message is long enough. */=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			vid =3D (*((u_int16_t *)msg->data));=0A=
 +			/* Sanity check the VLAN ID value. */=0A=
 +			if (vid & ~EVL_VLID_MASK) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			/* Check that hook exists and is active. */=0A=
 +			hook =3D priv->vlan_hook[vid];=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +			if (EVL_VLANOFTAG(hook_data) !=3D vid)=0A=
 +				printf("%s: NGM_VLAN_DEL_VID_FLT: Invalid VID Hook =3D %us, must =
 be: %us\n", __func__, (u_int16_t)EVL_VLANOFTAG(hook_data), vid);=0A=
 +#endif=0A=
 +			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[vid] =3D NULL;=0A=
  			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
  			break;=0A=
  		case NGM_VLAN_GET_TABLE:=0A=
 +			/* calculate vlans */=0A=
 +			vlan_count =3D 0;=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				if (priv->vlan_hook[i] !=3D NULL=0A=
 +				   && NG_HOOK_IS_VALID(priv->vlan_hook[i]))=0A=
 +					vlan_count ++;=0A=
 +			}=0A=
 +=0A=
 +			/* allocate memory for responce */=0A=
  			NG_MKRESPONSE(resp, msg, sizeof(*t) +=0A=
 -			    priv->nent * sizeof(*t->filter), M_NOWAIT);=0A=
 +			    vlan_count * sizeof(*t->filter), M_NOWAIT);=0A=
  			if (resp =3D=3D NULL) {=0A=
  				error =3D ENOMEM;=0A=
  				break;=0A=
  			}=0A=
 +=0A=
 +			/* pack data to responce */=0A=
  			t =3D (struct ng_vlan_table *)resp->data;=0A=
 -			t->n =3D priv->nent;=0A=
 +			t->n =3D 0;=0A=
  			vf =3D &t->filter[0];=0A=
 -			for (i =3D 0; i < HASHSIZE; i++) {=0A=
 -				LIST_FOREACH(f, &priv->hashtable[i], next) {=0A=
 -					vf->vlan =3D f->vlan;=0A=
 -					strncpy(vf->hook, NG_HOOK_NAME(f->hook),=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				hook =3D priv->vlan_hook[i];=0A=
 +				if (hook =3D=3D NULL=0A=
 +				   || NG_HOOK_NOT_VALID(hook))=0A=
 +					continue;=0A=
 +				hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +				if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)=0A=
 +					continue;=0A=
 +#ifdef NETGRAPH_DEBUG=0A=
 +				if (EVL_VLANOFTAG(hook_data) !=3D i)=0A=
 +					printf("%s: NGM_VLAN_GET_TABLE: hook %s VID =3D %us, must be: =
 %i\n", __func__, NG_HOOK_NAME(hook), =
 (u_int16_t)EVL_VLANOFTAG(hook_data), i);=0A=
 +#endif=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +				vf->vlan =3D i;=0A=
 +#endif=0A=
 +				vf->vid =3D i;=0A=
 +				vf->pcp =3D EVL_PRIOFTAG(hook_data);=0A=
 +				vf->cfi =3D EVL_CFIOFTAG(hook_data);=0A=
 +				strncpy(vf->hook_name, NG_HOOK_NAME(hook),=0A=
  					    NG_HOOKSIZ);=0A=
 -					vf++;=0A=
 -				}=0A=
 +				vf ++;=0A=
 +				t->n ++;=0A=
 +			}=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_DECAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->decap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_DECAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->decap_enable =3D (*((u_int32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((u_int32_t *)resp->data)) =3D priv->encap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_enable =3D (*((u_int32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP_PROTO:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(u_int16_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
  			}=0A=
 +			(*((u_int16_t *)resp->data)) =3D ntohs(priv->encap_proto);=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP_PROTO:=0A=
 +			if (msg->header.arglen !=3D sizeof(u_int16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_proto =3D htons((*((u_int16_t *)msg->data)));=0A=
  			break;=0A=
  		default:		/* Unknown command. */=0A=
  			error =3D EINVAL;=0A=
 @@ -300,8 +468,6 @@=0A=
  	case NGM_FLOW_COOKIE:=0A=
  	    {=0A=
  		struct ng_mesg *copy;=0A=
 -		struct filterhead *chain;=0A=
 -		struct filter *f;=0A=
  =0A=
  		/*=0A=
  		 * Flow control messages should come only=0A=
 @@ -312,17 +478,16 @@=0A=
  			break;=0A=
  		if (lasthook !=3D priv->downstream_hook)=0A=
  			break;=0A=
 -=0A=
  		/* Broadcast the event to all uplinks. */=0A=
 -		for (i =3D 0, chain =3D priv->hashtable; i < HASHSIZE;=0A=
 -		    i++, chain++)=0A=
 -		LIST_FOREACH(f, chain, next) {=0A=
 +		for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +			if (priv->vlan_hook[i] =3D=3D NULL)=0A=
 +				continue;=0A=
 +=0A=
  			NG_COPYMESSAGE(copy, msg, M_NOWAIT);=0A=
  			if (copy =3D=3D NULL)=0A=
 -				continue;=0A=
 -			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);=0A=
 +					continue;=0A=
 +			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);=0A=
  		}=0A=
 -=0A=
  		break;=0A=
  	    }=0A=
  	default:			/* Unknown type cookie. */=0A=
 @@ -340,92 +505,155 @@=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
  	struct ether_header *eh;=0A=
  	struct ether_vlan_header *evl =3D NULL;=0A=
 -	int error;=0A=
 -	u_int16_t vlan;=0A=
 +	int error, encapsulate =3D 0;=0A=
 +	uintptr_t hook_data;=0A=
 +	u_int16_t vid, eth_vtag;=0A=
  	struct mbuf *m;=0A=
 -	struct filter *f;=0A=
 +	hook_p dst_hook;=0A=
 +=0A=
  =0A=
 -	/* Make sure we have an entire header. */=0A=
  	NGI_GET_M(item, m);=0A=
 -	if (m->m_len < sizeof(*eh) &&=0A=
 -	    (m =3D m_pullup(m, sizeof(*eh))) =3D=3D NULL) {=0A=
 -		NG_FREE_ITEM(item);=0A=
 -		return (EINVAL);=0A=
 -	}=0A=
 +=0A=
 +	/* Make sure we have an entire header. */=0A=
 +	error =3D m_chk(&m, ETHER_HDR_LEN);=0A=
 +	if (error !=3D 0)=0A=
 +		goto mchk_err;=0A=
 +=0A=
  	eh =3D mtod(m, struct ether_header *);=0A=
  	if (hook =3D=3D priv->downstream_hook) {=0A=
  		/*=0A=
  		 * If from downstream, select between a match hook=0A=
  		 * or the nomatch hook.=0A=
  		 */=0A=
 -		if (m->m_flags & M_VLANTAG ||=0A=
 -		    eh->ether_type =3D=3D htons(ETHERTYPE_VLAN)) {=0A=
 +		dst_hook =3D priv->nomatch_hook;=0A=
 +		if (m->m_flags & M_VLANTAG=0A=
 +		    || eh->ether_type =3D=3D priv->encap_proto) {=0A=
  			if (m->m_flags & M_VLANTAG) {=0A=
  				/*=0A=
  				 * Packet is tagged, m contains a normal=0A=
  				 * Ethernet frame; tag is stored out-of-band.=0A=
  				 */=0A=
 -				vlan =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 -			} else {=0A=
 -				if (m->m_len < sizeof(*evl) &&=0A=
 -				    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL) {=0A=
 -					NG_FREE_ITEM(item);=0A=
 -					return (EINVAL);=0A=
 -				}=0A=
 +				/* evl =3D NULL; */=0A=
 +				vid =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 +			} else { /* eh->ether_type =3D=3D priv->encap_proto */=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +				if (error !=3D 0)=0A=
 +					goto mchk_err;=0A=
 +=0A=
  				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -				vlan =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 +				vid =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
  			}=0A=
 -			if ((f =3D ng_vlan_findentry(priv, vlan)) !=3D NULL) {=0A=
 -				if (m->m_flags & M_VLANTAG) {=0A=
 -					m->m_pkthdr.ether_vtag =3D 0;=0A=
 -					m->m_flags &=3D ~M_VLANTAG;=0A=
 +=0A=
 +			if (priv->vlan_hook[vid] !=3D NULL) {=0A=
 +				/* vlan filter: allways remove all vlan tags and decapsulate packet =
 */=0A=
 +				dst_hook =3D priv->vlan_hook[vid];=0A=
 +				m->m_pkthdr.ether_vtag =3D 0;=0A=
 +				m->m_flags &=3D ~M_VLANTAG;=0A=
 +			} else if (evl !=3D NULL) { /* nomatch_hook and encapsulated packet =
 */=0A=
 +				if (priv->decap_enable =3D=3D 0) {=0A=
 +					evl =3D NULL;=0A=
  				} else {=0A=
 -					evl->evl_encap_proto =3D evl->evl_proto;=0A=
 -					bcopy(mtod(m, caddr_t),=0A=
 -					    mtod(m, caddr_t) +=0A=
 -					    ETHER_VLAN_ENCAP_LEN,=0A=
 -					    ETHER_HDR_LEN);=0A=
 -					m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
 +					m->m_pkthdr.ether_vtag =3D ntohs(evl->evl_tag);=0A=
 +					m->m_flags |=3D M_VLANTAG;=0A=
  				}=0A=
  			}=0A=
 -		} else=0A=
 -			f =3D NULL;=0A=
 -		if (f !=3D NULL)=0A=
 -			NG_FWD_NEW_DATA(error, item, f->hook, m);=0A=
 -		else=0A=
 -			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);=0A=
 +=0A=
 +			if (evl !=3D NULL) {=0A=
 +				/* =0A=
 +				 * decapsulate=0A=
 +				 * move DstMAC and SrcMAC to ETHER_TYPE=0A=
 +				 * before: [dst_mac] [src_mac] [ether_type_encap(TPID)] =
 [PCP/CFI/VID] [ether_type] [payload]=0A=
 +				 *         |-----------------| =
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |---------------------|=0A=
 +				 * after: [free space] [dst_mac] [src_mac] [ether_type] [payload]=0A=
 +				 *                     |-----------------| |---------------------|=0A=
 +				 */=0A=
 +				bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +					(ETHER_ADDR_LEN * 2));=0A=
 +				m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
 +			}=0A=
 +		}=0A=
 +=0A=
 +		if (dst_hook =3D=3D NULL) {=0A=
 +			error =3D ENETDOWN;=0A=
 +			goto drop;=0A=
 +		}=0A=
  	} else {=0A=
  		/*=0A=
  		 * It is heading towards the downstream.=0A=
  		 * If from nomatch, pass it unmodified.=0A=
  		 * Otherwise, do the VLAN encapsulation.=0A=
  		 */=0A=
 -		if (hook !=3D priv->nomatch_hook) {=0A=
 -			if ((f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				NG_FREE_M(m);=0A=
 -				return (EOPNOTSUPP);=0A=
 +		dst_hook =3D priv->downstream_hook;=0A=
 +		if (dst_hook =3D=3D NULL) {=0A=
 +			error =3D ENETDOWN;=0A=
 +			goto drop;=0A=
 +		}=0A=
 +		if (hook !=3D priv->nomatch_hook) {/* filter hook */=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				/*=0A=
 +				 * packet from hook not in filter=0A=
 +				 * call addfilter for this hook to fix=0A=
 +				 */=0A=
 +				error =3D EOPNOTSUPP;=0A=
 +				goto drop;=0A=
  			}=0A=
 -			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 -			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 -			if (m =3D=3D NULL || (m->m_len < sizeof(*evl) &&=0A=
 -			    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL)) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				return (ENOMEM);=0A=
 +			eth_vtag =3D (hook_data & VLAN_TAG_MASK);=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) !=3D 0) {=0A=
 +				encapsulate =3D 1;=0A=
 +			} else {=0A=
 +				/* just set packet header tag */=0A=
 +				m->m_flags |=3D M_VLANTAG;=0A=
 +				m->m_pkthdr.ether_vtag =3D eth_vtag;=0A=
 +			}=0A=
 +		} else { /* nomatch_hook */=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) !=3D 0=0A=
 +			    && m->m_flags & M_VLANTAG) {=0A=
 +				/* encapsulate tagged packet */=0A=
 +				eth_vtag =3D m->m_pkthdr.ether_vtag;=0A=
 +				m->m_pkthdr.ether_vtag =3D 0;=0A=
 +				m->m_flags &=3D ~M_VLANTAG;=0A=
 +				encapsulate =3D 1;=0A=
  			}=0A=
 +		}=0A=
 +=0A=
 +		if (encapsulate !=3D 0) {=0A=
  			/*=0A=
  			 * Transform the Ethernet header into an Ethernet header=0A=
  			 * with 802.1Q encapsulation.=0A=
 +			 * mod of: ether_vlanencap =0A=
 +			 */=0A=
 +			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 +			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 +			if (m =3D=3D NULL)=0A=
 +				error =3D ENOMEM;=0A=
 +			else=0A=
 +				error =3D m_chk(&m, (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN));=0A=
 +			if (error !=3D 0)=0A=
 +				goto mchk_err;=0A=
 +=0A=
 +			/* move DstMAC and SrcMAC from ETHER_TYPE=0A=
 +			 * before: [free - prepended space] [dst_mac] [src_mac] [ether_type] =
 [payload]=0A=
 +			 *          <<<<<<<<<<<<<<<<<<<<<<  |-----------------| =
 |--------------------|=0A=
 +			 * after: [dst_mac] [src_mac] [ether_type_encap(TPID)] [PCP/CFI/VID] =
 [ether_type] [payload]=0A=
 +			 *        |-----------------| |----------- inserted tag -----------| =
 |--------------------| =0A=
  			 */=0A=
 -			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,=0A=
 -			    mtod(m, char *), ETHER_HDR_LEN);=0A=
  			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -			evl->evl_proto =3D evl->evl_encap_proto;=0A=
 -			evl->evl_encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 -			evl->evl_tag =3D htons(f->vlan);=0A=
 +			bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +				(char *)evl, (ETHER_ADDR_LEN * 2));=0A=
 +			evl->evl_encap_proto =3D priv->encap_proto;=0A=
 +			evl->evl_tag =3D htons(eth_vtag);=0A=
  		}=0A=
 -		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);=0A=
  	}=0A=
 +=0A=
 +	/* send packet */=0A=
 +	NG_FWD_NEW_DATA(error, item, dst_hook, m);=0A=
 +	return (error);=0A=
 +=0A=
 +drop:=0A=
 +	m_freem(m);=0A=
 +mchk_err:=0A=
 +	NG_FREE_ITEM(item);=0A=
  	return (error);=0A=
  }=0A=
  =0A=
 @@ -444,7 +672,7 @@=0A=
  ng_vlan_disconnect(hook_p hook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
 -	struct filter *f;=0A=
 +	uintptr_t hook_data;=0A=
  =0A=
  	if (hook =3D=3D priv->downstream_hook)=0A=
  		priv->downstream_hook =3D NULL;=0A=
 @@ -452,11 +680,9 @@=0A=
  		priv->nomatch_hook =3D NULL;=0A=
  	else {=0A=
  		/* Purge a rule that refers to this hook. */=0A=
 -		if ((f =3D NG_HOOK_PRIVATE(hook)) !=3D NULL) {=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
 -		}=0A=
 +		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +		if (IS_HOOK_VLAN_SET(hook_data))=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
  	}=0A=
  	NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
  	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) =3D=3D 0) &&=0A=
 --- ./ng_vlan.h.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ./ng_vlan.h	2012-01-23 07:34:26.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -31,6 +32,10 @@=0A=
  #ifndef _NETGRAPH_NG_VLAN_H_=0A=
  #define	_NETGRAPH_NG_VLAN_H_=0A=
  =0A=
 +#define NG_VLAN_USE_OLD_VLAN_NAME 1 /* using "vlan" in addfilter and =
 gettable messages. 2012.01 */=0A=
 +=0A=
 +=0A=
 +=0A=
  /* Node type name and magic cookie. */=0A=
  #define	NG_VLAN_NODE_TYPE	"vlan"=0A=
  #define	NGM_VLAN_COOKIE		1068486472=0A=
 @@ -43,21 +48,51 @@=0A=
  enum {=0A=
  	NGM_VLAN_ADD_FILTER =3D 1,=0A=
  	NGM_VLAN_DEL_FILTER,=0A=
 -	NGM_VLAN_GET_TABLE=0A=
 +	NGM_VLAN_DEL_VID_FLT,=0A=
 +	NGM_VLAN_GET_TABLE,=0A=
 +	NGM_VLAN_GET_DECAP,=0A=
 +	NGM_VLAN_SET_DECAP,=0A=
 +	NGM_VLAN_GET_ENCAP,=0A=
 +	NGM_VLAN_SET_ENCAP,=0A=
 +	NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	NGM_VLAN_SET_ENCAP_PROTO,=0A=
  };=0A=
  =0A=
 +#define VLAN_ENCAP_FROM_FILTER	0x00000001=0A=
 +#define VLAN_ENCAP_FROM_NOMATCH	0x00000002=0A=
 +=0A=
 +=0A=
  /* For NGM_VLAN_ADD_FILTER control message. */=0A=
  struct ng_vlan_filter {=0A=
 -	char		hook[NG_HOOKSIZ];=0A=
 -	u_int16_t	vlan;=0A=
 -};	=0A=
 +	char		hook_name[NG_HOOKSIZ];=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +	u_int16_t	vlan;	/* VLAN -  VLAN Identifier - same as vid, oldname, =
 deprecated */=0A=
 +#endif=0A=
 +	u_int16_t	vid;	/* VID - VLAN Identifier */=0A=
 +	u_int8_t	pcp;	/* PCP - Priority Code Point */=0A=
 +	u_int8_t	cfi;	/* CFI - Canonical Format Indicator */=0A=
 +};=0A=
  =0A=
  /* Keep this in sync with the above structure definition.  */=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
  #define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 -	{ "hook",	&ng_parse_hookbuf_type  },		\=0A=
 -	{ "vlan",	&ng_parse_uint16_type   },		\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vlan",	&ng_parse_uint16_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
  	{ NULL }						\=0A=
  }=0A=
 +#else=0A=
 +#define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
 +	{ NULL }						\=0A=
 +}=0A=
 +#endif=0A=
 +=0A=
  =0A=
  /* Structure returned by NGM_VLAN_GET_TABLE. */=0A=
  struct ng_vlan_table {=0A=
 
 ------=_NextPart_000_0017_01CCDAFC.44F8C6B0--
 

From: rozhuk.im@gmail.com
To: <bug-followup@FreeBSD.org>,
	<rozhuk.im@gmail.com>
Cc: "'Adrian Chadd'" <adrian@freebsd.org>,
	"'Gleb Smirnoff'" <glebius@freebsd.org>
Subject: Re: kern/161908: [netgraph] [patch] ng_vlan update for QinQ support
Date: Sat, 11 Feb 2012 03:37:44 +0900

 This is a multi-part message in MIME format.
 
 ------=_NextPart_000_00F4_01CCE86E.8567F110
 Content-Type: text/plain;
 	charset="US-ASCII"
 Content-Transfer-Encoding: 7bit
 
 Fix style(9) issues.
 
 
 ------=_NextPart_000_00F4_01CCE86E.8567F110
 Content-Type: application/octet-stream;
 	name="ng_vlan.patch_v4_2"
 Content-Transfer-Encoding: quoted-printable
 Content-Disposition: attachment;
 	filename="ng_vlan.patch_v4_2"
 
 --- ng_vlan.h.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ng_vlan.h	2012-02-10 02:23:07.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -31,6 +32,9 @@=0A=
  #ifndef _NETGRAPH_NG_VLAN_H_=0A=
  #define	_NETGRAPH_NG_VLAN_H_=0A=
  =0A=
 +/* Using "vlan" in addfilter and gettable messages. 2012.01 */=0A=
 +#define NG_VLAN_USE_OLD_VLAN_NAME 1=0A=
 +=0A=
  /* Node type name and magic cookie. */=0A=
  #define	NG_VLAN_NODE_TYPE	"vlan"=0A=
  #define	NGM_VLAN_COOKIE		1068486472=0A=
 @@ -43,25 +47,54 @@=0A=
  enum {=0A=
  	NGM_VLAN_ADD_FILTER =3D 1,=0A=
  	NGM_VLAN_DEL_FILTER,=0A=
 -	NGM_VLAN_GET_TABLE=0A=
 +	NGM_VLAN_GET_TABLE,=0A=
 +	NGM_VLAN_DEL_VID_FLT,=0A=
 +	NGM_VLAN_GET_DECAP,=0A=
 +	NGM_VLAN_SET_DECAP,=0A=
 +	NGM_VLAN_GET_ENCAP,=0A=
 +	NGM_VLAN_SET_ENCAP,=0A=
 +	NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	NGM_VLAN_SET_ENCAP_PROTO,=0A=
  };=0A=
  =0A=
 +#define VLAN_ENCAP_FROM_FILTER	0x00000001=0A=
 +#define VLAN_ENCAP_FROM_NOMATCH	0x00000002=0A=
 +=0A=
  /* For NGM_VLAN_ADD_FILTER control message. */=0A=
  struct ng_vlan_filter {=0A=
 -	char		hook[NG_HOOKSIZ];=0A=
 -	u_int16_t	vlan;=0A=
 -};	=0A=
 +	char		hook_name[NG_HOOKSIZ];=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +	uint16_t	vlan;	/* VLAN - same as vid, oldname, deprecated. */=0A=
 +#endif=0A=
 +	uint16_t	vid;	/* VID - VLAN Identifier. */=0A=
 +	uint8_t		pcp;	/* PCP - Priority Code Point. */=0A=
 +	uint8_t		cfi;	/* CFI - Canonical Format Indicator. */=0A=
 +};=0A=
  =0A=
  /* Keep this in sync with the above structure definition.  */=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
  #define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 -	{ "hook",	&ng_parse_hookbuf_type  },		\=0A=
 -	{ "vlan",	&ng_parse_uint16_type   },		\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vlan",	&ng_parse_uint16_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
  	{ NULL }						\=0A=
  }=0A=
 +#else=0A=
 +#define	NG_VLAN_FILTER_FIELDS	{				\=0A=
 +	{ "hook",	&ng_parse_hookbuf_type	},		\=0A=
 +	{ "vid",	&ng_parse_uint16_type	},		\=0A=
 +	{ "pcp",	&ng_parse_uint8_type	},		\=0A=
 +	{ "cfi",	&ng_parse_uint8_type	},		\=0A=
 +	{ NULL }						\=0A=
 +}=0A=
 +#endif=0A=
 +=0A=
  =0A=
  /* Structure returned by NGM_VLAN_GET_TABLE. */=0A=
  struct ng_vlan_table {=0A=
 -	u_int32_t	n;=0A=
 +	uint32_t	n;=0A=
  	struct ng_vlan_filter filter[];=0A=
  };=0A=
  =0A=
 --- ng_vlan.c.orig	2011-09-23 09:51:37.000000000 +0900=0A=
 +++ ng_vlan.c	2012-02-11 03:29:16.000000000 +0900=0A=
 @@ -1,5 +1,6 @@=0A=
  /*-=0A=
   * Copyright (c) 2003 IPNET Internet Communication Company=0A=
 + * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>=0A=
   * All rights reserved.=0A=
   *=0A=
   * Redistribution and use in source and binary forms, with or without=0A=
 @@ -46,6 +47,22 @@=0A=
  #include <netgraph/ng_vlan.h>=0A=
  #include <netgraph/netgraph.h>=0A=
  =0A=
 +struct ng_vlan_private {=0A=
 +	hook_p		downstream_hook;=0A=
 +	hook_p		nomatch_hook;=0A=
 +	uint32_t	decap_enable;=0A=
 +	uint32_t	encap_enable;=0A=
 +	uint16_t	encap_proto;=0A=
 +	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];=0A=
 +};=0A=
 +typedef struct ng_vlan_private *priv_p;=0A=
 +=0A=
 +#define ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)=0A=
 +#define VLAN_TAG_MASK	0xFFFF=0A=
 +#define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))=0A=
 +#define IS_HOOK_VLAN_SET(hdata) \=0A=
 +	    ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) =3D=3D =
 HOOK_VLAN_TAG_SET_MASK)=0A=
 +=0A=
  static ng_constructor_t	ng_vlan_constructor;=0A=
  static ng_rcvmsg_t	ng_vlan_rcvmsg;=0A=
  static ng_shutdown_t	ng_vlan_shutdown;=0A=
 @@ -66,7 +83,7 @@=0A=
      const u_char *start, const u_char *buf)=0A=
  {=0A=
  	const struct ng_vlan_table *const table =3D=0A=
 -	    (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));=0A=
 +	    (const struct ng_vlan_table *)(buf - sizeof(uint32_t));=0A=
  =0A=
  	return table->n;=0A=
  }=0A=
 @@ -110,6 +127,55 @@=0A=
  	  NULL,=0A=
  	  &ng_vlan_table_type=0A=
  	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_DEL_VID_FLT,=0A=
 +	  "delvidflt",=0A=
 +	  &ng_parse_uint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_DECAP,=0A=
 +	  "getdecap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_DECAP,=0A=
 +	  "setdecap",=0A=
 +	  &ng_parse_hint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP,=0A=
 +	  "getencap",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint32_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP,=0A=
 +	  "setencap",=0A=
 +	  &ng_parse_hint32_type,=0A=
 +	  NULL=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_GET_ENCAP_PROTO,=0A=
 +	  "getencapproto",=0A=
 +	  NULL,=0A=
 +	  &ng_parse_hint16_type=0A=
 +	},=0A=
 +	{=0A=
 +	  NGM_VLAN_COOKIE,=0A=
 +	  NGM_VLAN_SET_ENCAP_PROTO,=0A=
 +	  "setencapproto",=0A=
 +	  &ng_parse_hint16_type,=0A=
 +	  NULL=0A=
 +	},=0A=
  	{ 0 }=0A=
  };=0A=
  =0A=
 @@ -126,44 +192,40 @@=0A=
  };=0A=
  NETGRAPH_INIT(vlan, &ng_vlan_typestruct);=0A=
  =0A=
 -struct filter {=0A=
 -	LIST_ENTRY(filter) next;=0A=
 -	u_int16_t	vlan;=0A=
 -	hook_p		hook;=0A=
 -};=0A=
  =0A=
 -#define	HASHSIZE	16=0A=
 -#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)=0A=
 -LIST_HEAD(filterhead, filter);=0A=
 +//**********************************************************************=
 **=0A=
 +//			HELPER STUFF=0A=
 +//**********************************************************************=
 **=0A=
  =0A=
 -typedef struct {=0A=
 -	hook_p		downstream_hook;=0A=
 -	hook_p		nomatch_hook;=0A=
 -	struct filterhead hashtable[HASHSIZE];=0A=
 -	u_int32_t	nent;=0A=
 -} *priv_p;=0A=
 -=0A=
 -static struct filter *=0A=
 -ng_vlan_findentry(priv_p priv, u_int16_t vlan)=0A=
 +static __inline int=0A=
 +m_chk(struct mbuf **mp, int len)=0A=
  {=0A=
 -	struct filterhead *chain =3D &priv->hashtable[HASH(vlan)];=0A=
 -	struct filter *f;=0A=
  =0A=
 -	LIST_FOREACH(f, chain, next)=0A=
 -		if (f->vlan =3D=3D vlan)=0A=
 -			return (f);=0A=
 -	return (NULL);=0A=
 +	if ((*mp)->m_pkthdr.len < len) {=0A=
 +		m_freem((*mp));=0A=
 +		(*mp) =3D NULL;=0A=
 +		return (EINVAL);=0A=
 +	}=0A=
 +	if ((*mp)->m_len < len && ((*mp) =3D m_pullup((*mp), len)) =3D=3D NULL)=0A=
 +		return (ENOBUFS);=0A=
 +=0A=
 +	return (0);=0A=
  }=0A=
  =0A=
 +=0A=
 +//**********************************************************************=
 **=0A=
 +//			NETGRAPH NODE STUFF=0A=
 +//**********************************************************************=
 **=0A=
 +=0A=
  static int=0A=
  ng_vlan_constructor(node_p node)=0A=
  {=0A=
  	priv_p priv;=0A=
 -	int i;=0A=
  =0A=
  	priv =3D malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);=0A=
 -	for (i =3D 0; i < HASHSIZE; i++)=0A=
 -		LIST_INIT(&priv->hashtable[i]);=0A=
 +	priv->decap_enable =3D 0;=0A=
 +	priv->encap_enable =3D VLAN_ENCAP_FROM_FILTER;=0A=
 +	priv->encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
  	NG_NODE_SET_PRIVATE(node, priv);=0A=
  	return (0);=0A=
  }=0A=
 @@ -191,13 +253,14 @@=0A=
  ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(node);=0A=
 -	int error =3D 0;=0A=
  	struct ng_mesg *msg, *resp =3D NULL;=0A=
  	struct ng_vlan_filter *vf;=0A=
 -	struct filter *f;=0A=
  	hook_p hook;=0A=
  	struct ng_vlan_table *t;=0A=
 -	int i;=0A=
 +	uintptr_t hook_data;=0A=
 +	int i, vlan_count;=0A=
 +	uint16_t vid;=0A=
 +	int error =3D 0;=0A=
  =0A=
  	NGI_GET_MSG(item, msg);=0A=
  	/* Deal with message according to cookie and command. */=0A=
 @@ -212,12 +275,23 @@=0A=
  			}=0A=
  			vf =3D (struct ng_vlan_filter *)msg->data;=0A=
  			/* Sanity check the VLAN ID value. */=0A=
 -			if (vf->vlan & ~EVL_VLID_MASK) {=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +			if (vf->vid =3D=3D 0 && vf->vid !=3D vf->vlan) {=0A=
 +				vf->vid =3D vf->vlan;=0A=
 +			} else if (vf->vid !=3D 0 && vf->vlan !=3D 0 &&=0A=
 +			    vf->vid !=3D vf->vlan) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +#endif=0A=
 +			if (vf->vid & ~EVL_VLID_MASK ||=0A=
 +			    vf->pcp & ~7 ||=0A=
 +			    vf->cfi & ~1) {=0A=
  				error =3D EINVAL;=0A=
  				break;=0A=
  			}=0A=
  			/* Check that a referenced hook exists. */=0A=
 -			hook =3D ng_findhook(node, vf->hook);=0A=
 +			hook =3D ng_findhook(node, vf->hook_name);=0A=
  			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
 @@ -229,30 +303,20 @@=0A=
  				break;=0A=
  			}=0A=
  			/* And is not already in service. */=0A=
 -			if (NG_HOOK_PRIVATE(hook) !=3D NULL) {=0A=
 +			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
  			/* Check we don't already trap this VLAN. */=0A=
 -			if (ng_vlan_findentry(priv, vf->vlan)) {=0A=
 +			if (priv->vlan_hook[vf->vid] !=3D NULL) {=0A=
  				error =3D EEXIST;=0A=
  				break;=0A=
  			}=0A=
 -			/* Create filter. */=0A=
 -			f =3D malloc(sizeof(*f),=0A=
 -			    M_NETGRAPH, M_NOWAIT | M_ZERO);=0A=
 -			if (f =3D=3D NULL) {=0A=
 -				error =3D ENOMEM;=0A=
 -				break;=0A=
 -			}=0A=
 -			/* Link filter and hook together. */=0A=
 -			f->hook =3D hook;=0A=
 -			f->vlan =3D vf->vlan;=0A=
 -			NG_HOOK_SET_PRIVATE(hook, f);=0A=
 -			/* Register filter in a hash table. */=0A=
 -			LIST_INSERT_HEAD(=0A=
 -			    &priv->hashtable[HASH(f->vlan)], f, next);=0A=
 -			priv->nent++;=0A=
 +			/* Link vlan and hook together. */=0A=
 +			NG_HOOK_SET_PRIVATE(hook,=0A=
 +			    (void *)(HOOK_VLAN_TAG_SET_MASK |=0A=
 +			    EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));=0A=
 +			priv->vlan_hook[vf->vid] =3D hook;=0A=
  			break;=0A=
  		case NGM_VLAN_DEL_FILTER:=0A=
  			/* Check that message is long enough. */=0A=
 @@ -262,37 +326,150 @@=0A=
  			}=0A=
  			/* Check that hook exists and is active. */=0A=
  			hook =3D ng_findhook(node, (char *)msg->data);=0A=
 -			if (hook =3D=3D NULL ||=0A=
 -			    (f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 +			if (hook =3D=3D NULL) {=0A=
  				error =3D ENOENT;=0A=
  				break;=0A=
  			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +=0A=
 +			KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D=3D hook,=0A=
 +			    ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook =3D %s\n",=0A=
 +			    __func__, (char *)msg->data));=0A=
 +=0A=
  			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
 +			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 +			break;=0A=
 +		case NGM_VLAN_DEL_VID_FLT:=0A=
 +			/* Check that message is long enough. */=0A=
 +			if (msg->header.arglen !=3D sizeof(uint16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			vid =3D (*((uint16_t *)msg->data));=0A=
 +			/* Sanity check the VLAN ID value. */=0A=
 +			if (vid & ~EVL_VLID_MASK) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			/* Check that hook exists and is active. */=0A=
 +			hook =3D priv->vlan_hook[vid];=0A=
 +			if (hook =3D=3D NULL) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				error =3D ENOENT;=0A=
 +				break;=0A=
 +			}=0A=
 +=0A=
 +			KASSERT(EVL_VLANOFTAG(hook_data) =3D=3D vid,=0A=
 +			    ("%s: NGM_VLAN_DEL_VID_FLT:"=0A=
 +			    " Invalid VID Hook =3D %us, must be: %us\n",=0A=
 +			    __func__, (uint16_t )EVL_VLANOFTAG(hook_data), vid));=0A=
 +=0A=
 +			/* Purge a rule that refers to this hook. */=0A=
 +			priv->vlan_hook[vid] =3D NULL;=0A=
  			NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
  			break;=0A=
  		case NGM_VLAN_GET_TABLE:=0A=
 +			/* Calculate vlans. */=0A=
 +			vlan_count =3D 0;=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				if (priv->vlan_hook[i] !=3D NULL &&=0A=
 +				    NG_HOOK_IS_VALID(priv->vlan_hook[i]))=0A=
 +					vlan_count ++;=0A=
 +			}=0A=
 +=0A=
 +			/* Allocate memory for responce. */=0A=
  			NG_MKRESPONSE(resp, msg, sizeof(*t) +=0A=
 -			    priv->nent * sizeof(*t->filter), M_NOWAIT);=0A=
 +			    vlan_count * sizeof(*t->filter), M_NOWAIT);=0A=
  			if (resp =3D=3D NULL) {=0A=
  				error =3D ENOMEM;=0A=
  				break;=0A=
  			}=0A=
 +=0A=
 +			/* Pack data to responce. */=0A=
  			t =3D (struct ng_vlan_table *)resp->data;=0A=
 -			t->n =3D priv->nent;=0A=
 +			t->n =3D 0;=0A=
  			vf =3D &t->filter[0];=0A=
 -			for (i =3D 0; i < HASHSIZE; i++) {=0A=
 -				LIST_FOREACH(f, &priv->hashtable[i], next) {=0A=
 -					vf->vlan =3D f->vlan;=0A=
 -					strncpy(vf->hook, NG_HOOK_NAME(f->hook),=0A=
 -					    NG_HOOKSIZ);=0A=
 -					vf++;=0A=
 -				}=0A=
 +			for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +				hook =3D priv->vlan_hook[i];=0A=
 +				if (hook =3D=3D NULL || NG_HOOK_NOT_VALID(hook))=0A=
 +					continue;=0A=
 +				hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +				if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0)=0A=
 +					continue;=0A=
 +=0A=
 +				KASSERT(EVL_VLANOFTAG(hook_data) =3D=3D i,=0A=
 +				    ("%s: NGM_VLAN_GET_TABLE:"=0A=
 +				    " hook %s VID =3D %us, must be: %i\n",=0A=
 +				    __func__, NG_HOOK_NAME(hook),=0A=
 +				    (uint16_t)EVL_VLANOFTAG(hook_data), i));=0A=
 +=0A=
 +#ifdef NG_VLAN_USE_OLD_VLAN_NAME=0A=
 +				vf->vlan =3D i;=0A=
 +#endif=0A=
 +				vf->vid =3D i;=0A=
 +				vf->pcp =3D EVL_PRIOFTAG(hook_data);=0A=
 +				vf->cfi =3D EVL_CFIOFTAG(hook_data);=0A=
 +				strncpy(vf->hook_name,=0A=
 +				    NG_HOOK_NAME(hook), NG_HOOKSIZ);=0A=
 +				vf ++;=0A=
 +				t->n ++;=0A=
  			}=0A=
  			break;=0A=
 -		default:		/* Unknown command. */=0A=
 +		case NGM_VLAN_GET_DECAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((uint32_t *)resp->data)) =3D priv->decap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_DECAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(uint32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->decap_enable =3D (*((uint32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((uint32_t *)resp->data)) =3D priv->encap_enable;=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP:=0A=
 +			if (msg->header.arglen !=3D sizeof(uint32_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_enable =3D (*((uint32_t *)msg->data));=0A=
 +			break;=0A=
 +		case NGM_VLAN_GET_ENCAP_PROTO:=0A=
 +			NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);=0A=
 +			if (resp =3D=3D NULL) {=0A=
 +				error =3D ENOMEM;=0A=
 +				break;=0A=
 +			}=0A=
 +			(*((uint16_t *)resp->data)) =3D ntohs(priv->encap_proto);=0A=
 +			break;=0A=
 +		case NGM_VLAN_SET_ENCAP_PROTO:=0A=
 +			if (msg->header.arglen !=3D sizeof(uint16_t)) {=0A=
 +				error =3D EINVAL;=0A=
 +				break;=0A=
 +			}=0A=
 +			priv->encap_proto =3D htons((*((uint16_t *)msg->data)));=0A=
 +			break;=0A=
 +		default: /* Unknown command. */=0A=
  			error =3D EINVAL;=0A=
  			break;=0A=
  		}=0A=
 @@ -300,8 +477,6 @@=0A=
  	case NGM_FLOW_COOKIE:=0A=
  	    {=0A=
  		struct ng_mesg *copy;=0A=
 -		struct filterhead *chain;=0A=
 -		struct filter *f;=0A=
  =0A=
  		/*=0A=
  		 * Flow control messages should come only=0A=
 @@ -312,20 +487,19 @@=0A=
  			break;=0A=
  		if (lasthook !=3D priv->downstream_hook)=0A=
  			break;=0A=
 -=0A=
  		/* Broadcast the event to all uplinks. */=0A=
 -		for (i =3D 0, chain =3D priv->hashtable; i < HASHSIZE;=0A=
 -		    i++, chain++)=0A=
 -		LIST_FOREACH(f, chain, next) {=0A=
 +		for (i =3D 0; i < (EVL_VLID_MASK + 1); i ++) {=0A=
 +			if (priv->vlan_hook[i] =3D=3D NULL)=0A=
 +				continue;=0A=
 +=0A=
  			NG_COPYMESSAGE(copy, msg, M_NOWAIT);=0A=
  			if (copy =3D=3D NULL)=0A=
  				continue;=0A=
 -			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);=0A=
 +			NG_SEND_MSG_HOOK(error, node, copy, priv->vlan_hook[i], 0);=0A=
  		}=0A=
 -=0A=
  		break;=0A=
  	    }=0A=
 -	default:			/* Unknown type cookie. */=0A=
 +	default: /* Unknown type cookie. */=0A=
  		error =3D EINVAL;=0A=
  		break;=0A=
  	}=0A=
 @@ -339,93 +513,159 @@=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
  	struct ether_header *eh;=0A=
 -	struct ether_vlan_header *evl =3D NULL;=0A=
 +	struct ether_vlan_header *evl;=0A=
  	int error;=0A=
 -	u_int16_t vlan;=0A=
 +	uintptr_t hook_data;=0A=
 +	uint16_t vid, eth_vtag;=0A=
  	struct mbuf *m;=0A=
 -	struct filter *f;=0A=
 +	hook_p dst_hook;=0A=
 +=0A=
  =0A=
 -	/* Make sure we have an entire header. */=0A=
  	NGI_GET_M(item, m);=0A=
 -	if (m->m_len < sizeof(*eh) &&=0A=
 -	    (m =3D m_pullup(m, sizeof(*eh))) =3D=3D NULL) {=0A=
 -		NG_FREE_ITEM(item);=0A=
 -		return (EINVAL);=0A=
 -	}=0A=
 +=0A=
 +	/* Make sure we have an entire header. */=0A=
 +	error =3D m_chk(&m, ETHER_HDR_LEN);=0A=
 +	if (error !=3D 0)=0A=
 +		goto mchk_err;=0A=
 +=0A=
  	eh =3D mtod(m, struct ether_header *);=0A=
  	if (hook =3D=3D priv->downstream_hook) {=0A=
  		/*=0A=
  		 * If from downstream, select between a match hook=0A=
  		 * or the nomatch hook.=0A=
  		 */=0A=
 -		if (m->m_flags & M_VLANTAG ||=0A=
 -		    eh->ether_type =3D=3D htons(ETHERTYPE_VLAN)) {=0A=
 -			if (m->m_flags & M_VLANTAG) {=0A=
 -				/*=0A=
 -				 * Packet is tagged, m contains a normal=0A=
 -				 * Ethernet frame; tag is stored out-of-band.=0A=
 -				 */=0A=
 -				vlan =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 -			} else {=0A=
 -				if (m->m_len < sizeof(*evl) &&=0A=
 -				    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL) {=0A=
 -					NG_FREE_ITEM(item);=0A=
 -					return (EINVAL);=0A=
 -				}=0A=
 -				evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -				vlan =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 -			}=0A=
 -			if ((f =3D ng_vlan_findentry(priv, vlan)) !=3D NULL) {=0A=
 -				if (m->m_flags & M_VLANTAG) {=0A=
 -					m->m_pkthdr.ether_vtag =3D 0;=0A=
 -					m->m_flags &=3D ~M_VLANTAG;=0A=
 -				} else {=0A=
 -					evl->evl_encap_proto =3D evl->evl_proto;=0A=
 -					bcopy(mtod(m, caddr_t),=0A=
 -					    mtod(m, caddr_t) +=0A=
 -					    ETHER_VLAN_ENCAP_LEN,=0A=
 -					    ETHER_HDR_LEN);=0A=
 -					m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
 -				}=0A=
 -			}=0A=
 -		} else=0A=
 -			f =3D NULL;=0A=
 -		if (f !=3D NULL)=0A=
 -			NG_FWD_NEW_DATA(error, item, f->hook, m);=0A=
 -		else=0A=
 -			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);=0A=
 +=0A=
 +		dst_hook =3D priv->nomatch_hook;=0A=
 +=0A=
 +		/* Skip packets without tag. */=0A=
 +		if ((m->m_flags & M_VLANTAG) =3D=3D 0 &&=0A=
 +		    eh->ether_type !=3D priv->encap_proto) {=0A=
 +			if (dst_hook =3D=3D NULL)=0A=
 +				goto net_down;=0A=
 +			goto send_packet;=0A=
 +		}=0A=
 +=0A=
 +		/* Process packets with tag. */=0A=
 +		if (m->m_flags & M_VLANTAG) {=0A=
 +			/*=0A=
 +			 * Packet is tagged, m contains a normal=0A=
 +			 * Ethernet frame; tag is stored out-of-band.=0A=
 +			 */=0A=
 +			evl =3D NULL;=0A=
 +			vid =3D EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);=0A=
 +		} else { /* eh->ether_type =3D=3D priv->encap_proto */=0A=
 +			error =3D m_chk(&m, ETHER_VLAN_HDR_LEN);=0A=
 +			if (error !=3D 0)=0A=
 +				goto mchk_err;=0A=
 +			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 +			vid =3D EVL_VLANOFTAG(ntohs(evl->evl_tag));=0A=
 +		}=0A=
 +=0A=
 +		if (priv->vlan_hook[vid] !=3D NULL) {=0A=
 +			/*=0A=
 +			 * VLAN filter: allways remove vlan tags and=0A=
 +			 * decapsulate packet.=0A=
 +			 */=0A=
 +			dst_hook =3D priv->vlan_hook[vid];=0A=
 +			if (evl =3D=3D NULL) { /* m->m_flags & M_VLANTAG */=0A=
 +				m->m_pkthdr.ether_vtag =3D 0;=0A=
 +				m->m_flags &=3D ~M_VLANTAG;=0A=
 +				goto send_packet;=0A=
 +			}=0A=
 +		} else { /* nomatch_hook */=0A=
 +			if (dst_hook =3D=3D NULL)=0A=
 +				goto net_down;=0A=
 +			if (evl =3D=3D NULL || priv->decap_enable =3D=3D 0)=0A=
 +				goto send_packet;=0A=
 +			/* Save tag out-of-band. */=0A=
 +			m->m_pkthdr.ether_vtag =3D ntohs(evl->evl_tag);=0A=
 +			m->m_flags |=3D M_VLANTAG;=0A=
 +		}=0A=
 +=0A=
 +		/*=0A=
 +		 * Decapsulate:=0A=
 +		 * TPID =3D ether type encap=0A=
 +		 * Move DstMAC and SrcMAC to ETHER_TYPE.=0A=
 +		 * Before: [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]=0A=
 +		 *         |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|=0A=
 +		 * After:  [free space ] [dmac] [smac] [ether_type] [payload]=0A=
 +		 *                       |-----------| |--------------------|=0A=
 +		 */=0A=
 +		bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +		    (ETHER_ADDR_LEN * 2));=0A=
 +		m_adj(m, ETHER_VLAN_ENCAP_LEN);=0A=
  	} else {=0A=
  		/*=0A=
  		 * It is heading towards the downstream.=0A=
  		 * If from nomatch, pass it unmodified.=0A=
  		 * Otherwise, do the VLAN encapsulation.=0A=
  		 */=0A=
 -		if (hook !=3D priv->nomatch_hook) {=0A=
 -			if ((f =3D NG_HOOK_PRIVATE(hook)) =3D=3D NULL) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				NG_FREE_M(m);=0A=
 -				return (EOPNOTSUPP);=0A=
 -			}=0A=
 -			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 -			/* M_PREPEND takes care of m_len and m_pkthdr.len. */=0A=
 -			if (m =3D=3D NULL || (m->m_len < sizeof(*evl) &&=0A=
 -			    (m =3D m_pullup(m, sizeof(*evl))) =3D=3D NULL)) {=0A=
 -				NG_FREE_ITEM(item);=0A=
 -				return (ENOMEM);=0A=
 +		dst_hook =3D priv->downstream_hook;=0A=
 +		if (dst_hook =3D=3D NULL)=0A=
 +			goto net_down;=0A=
 +		if (hook !=3D priv->nomatch_hook) {/* Filter hook. */=0A=
 +			hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +			if (IS_HOOK_VLAN_SET(hook_data) =3D=3D 0) {=0A=
 +				/*=0A=
 +				 * Packet from hook not in filter=0A=
 +				 * call addfilter for this hook to fix.=0A=
 +				 */=0A=
 +				error =3D EOPNOTSUPP;=0A=
 +				goto drop;=0A=
  			}=0A=
 -			/*=0A=
 -			 * Transform the Ethernet header into an Ethernet header=0A=
 -			 * with 802.1Q encapsulation.=0A=
 -			 */=0A=
 -			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,=0A=
 -			    mtod(m, char *), ETHER_HDR_LEN);=0A=
 -			evl =3D mtod(m, struct ether_vlan_header *);=0A=
 -			evl->evl_proto =3D evl->evl_encap_proto;=0A=
 -			evl->evl_encap_proto =3D htons(ETHERTYPE_VLAN);=0A=
 -			evl->evl_tag =3D htons(f->vlan);=0A=
 +			eth_vtag =3D (hook_data & VLAN_TAG_MASK);=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) =3D=3D 0) {=0A=
 +				/* Just set packet header tag and send. */=0A=
 +				m->m_flags |=3D M_VLANTAG;=0A=
 +				m->m_pkthdr.ether_vtag =3D eth_vtag;=0A=
 +				goto send_packet;=0A=
 +			}=0A=
 +		} else { /* nomatch_hook */=0A=
 +			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) =3D=3D 0 ||=0A=
 +			    (m->m_flags & M_VLANTAG) =3D=3D 0)=0A=
 +				goto send_packet;=0A=
 +			/* Encapsulate tagged packet. */=0A=
 +			eth_vtag =3D m->m_pkthdr.ether_vtag;=0A=
 +			m->m_pkthdr.ether_vtag =3D 0;=0A=
 +			m->m_flags &=3D ~M_VLANTAG;=0A=
  		}=0A=
 -		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);=0A=
 +=0A=
 +		/*=0A=
 +		 * Transform the Ethernet header into an Ethernet header=0A=
 +		 * with 802.1Q encapsulation.=0A=
 +		 * Mod of: ether_vlanencap.=0A=
 +		 *=0A=
 +		 * TPID =3D ether type encap=0A=
 +		 * Move DstMAC and SrcMAC from ETHER_TYPE.=0A=
 +		 * Before: [free space ] [dmac] [smac] [ether_type] [payload]=0A=
 +		 *         <<<<<<<<<<<<< |-----------| |--------------------|=0A=
 +		 * After:  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]=0A=
 +		 *         |-----------| |-- inserted tag --| |--------------------|=0A=
 +		 */=0A=
 +		M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);=0A=
 +		if (m =3D=3D NULL)=0A=
 +			error =3D ENOMEM;=0A=
 +		else=0A=
 +			error =3D m_chk(&m, ETHER_VLAN_HDR_LEN);=0A=
 +		if (error !=3D 0)=0A=
 +			goto mchk_err;=0A=
 +=0A=
 +		evl =3D mtod(m, struct ether_vlan_header *);=0A=
 +		bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),=0A=
 +		    (char *)evl, (ETHER_ADDR_LEN * 2));=0A=
 +		evl->evl_encap_proto =3D priv->encap_proto;=0A=
 +		evl->evl_tag =3D htons(eth_vtag);=0A=
  	}=0A=
 +=0A=
 +send_packet:=0A=
 +	NG_FWD_NEW_DATA(error, item, dst_hook, m);=0A=
 +	return (error);=0A=
 +net_down:=0A=
 +	error =3D ENETDOWN;=0A=
 +drop:=0A=
 +	m_freem(m);=0A=
 +mchk_err:=0A=
 +	NG_FREE_ITEM(item);=0A=
  	return (error);=0A=
  }=0A=
  =0A=
 @@ -444,7 +684,7 @@=0A=
  ng_vlan_disconnect(hook_p hook)=0A=
  {=0A=
  	const priv_p priv =3D NG_NODE_PRIVATE(NG_HOOK_NODE(hook));=0A=
 -	struct filter *f;=0A=
 +	uintptr_t hook_data;=0A=
  =0A=
  	if (hook =3D=3D priv->downstream_hook)=0A=
  		priv->downstream_hook =3D NULL;=0A=
 @@ -452,11 +692,9 @@=0A=
  		priv->nomatch_hook =3D NULL;=0A=
  	else {=0A=
  		/* Purge a rule that refers to this hook. */=0A=
 -		if ((f =3D NG_HOOK_PRIVATE(hook)) !=3D NULL) {=0A=
 -			LIST_REMOVE(f, next);=0A=
 -			priv->nent--;=0A=
 -			free(f, M_NETGRAPH);=0A=
 -		}=0A=
 +		hook_data =3D (uintptr_t)NG_HOOK_PRIVATE(hook);=0A=
 +		if (IS_HOOK_VLAN_SET(hook_data))=0A=
 +			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] =3D NULL;=0A=
  	}=0A=
  	NG_HOOK_SET_PRIVATE(hook, NULL);=0A=
  	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) =3D=3D 0) &&=0A=
 
 ------=_NextPart_000_00F4_01CCE86E.8567F110--
 

From: Adrian Chadd <adrian@freebsd.org>
To: Rozhuk.IM@gmail.com
Cc: bug-followup@freebsd.org, Gleb Smirnoff <glebius@freebsd.org>
Subject: Re: kern/161908: [netgraph] [patch] ng_vlan update for QinQ support
Date: Sat, 11 Feb 2012 11:42:22 -0800

 Gleb, how's that look to you?
 
 
 
 Adrian
 
 
 On 10 February 2012 10:37,  <rozhuk.im@gmail.com> wrote:
 > Fix style(9) issues.
 >

From: dfilter@FreeBSD.ORG (dfilter service)
To: bug-followup@FreeBSD.org
Cc:  
Subject: Re: kern/161908: commit references a PR
Date: Sun, 11 Mar 2012 19:09:10 +0000 (UTC)

 Author: adrian
 Date: Sun Mar 11 19:08:56 2012
 New Revision: 232825
 URL: http://svn.freebsd.org/changeset/base/232825
 
 Log:
   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>
 
 Modified:
   head/sys/netgraph/ng_vlan.c
   head/sys/netgraph/ng_vlan.h
 
 Modified: head/sys/netgraph/ng_vlan.c
 ==============================================================================
 --- head/sys/netgraph/ng_vlan.c	Sun Mar 11 17:35:12 2012	(r232824)
 +++ head/sys/netgraph/ng_vlan.c	Sun Mar 11 19:08:56 2012	(r232825)
 @@ -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_c
  	  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,
  			}
  			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,
  				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,
  			}
  			/* 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;
 +		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;
 -		default:		/* Unknown command. */
 +		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,
  	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,
  			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);
 -				}
 -			}
 -		} 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);
 +
 +		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 { /* 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);
 -			}
 -			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);
 +		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;
  			}
 -			/*
 -			 * 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);
 +			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;
 +			}
 +		} 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) &&
 
 Modified: head/sys/netgraph/ng_vlan.h
 ==============================================================================
 --- head/sys/netgraph/ng_vlan.h	Sun Mar 11 17:35:12 2012	(r232824)
 +++ head/sys/netgraph/ng_vlan.h	Sun Mar 11 19:08:56 2012	(r232825)
 @@ -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 {
 _______________________________________________
 svn-src-all@freebsd.org mailing list
 http://lists.freebsd.org/mailman/listinfo/svn-src-all
 To unsubscribe, send any mail to "svn-src-all-unsubscribe@freebsd.org"
 
>Unformatted:
