Skip site navigation (1)Skip section navigation (2)
Date:      Sun, 11 Mar 2012 19:08:56 +0000 (UTC)
From:      Adrian Chadd <adrian@FreeBSD.org>
To:        src-committers@freebsd.org, svn-src-all@freebsd.org, svn-src-head@freebsd.org
Subject:   svn commit: r232825 - head/sys/netgraph
Message-ID:  <201203111908.q2BJ8uUY022869@svn.freebsd.org>

next in thread | raw e-mail | index | archive | help
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 {



Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?201203111908.q2BJ8uUY022869>