xref: /freebsd/sys/netgraph/ng_vlan.c (revision bbf53c35ea2f92cc742f847870b67d0599afcdbd)
1407ea290SRuslan Ermilov /*-
2407ea290SRuslan Ermilov  * Copyright (c) 2003 IPNET Internet Communication Company
3*bbf53c35SAdrian Chadd  * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>
4407ea290SRuslan Ermilov  * All rights reserved.
5407ea290SRuslan Ermilov  *
6407ea290SRuslan Ermilov  * Redistribution and use in source and binary forms, with or without
7407ea290SRuslan Ermilov  * modification, are permitted provided that the following conditions
8407ea290SRuslan Ermilov  * are met:
9407ea290SRuslan Ermilov  * 1. Redistributions of source code must retain the above copyright
10407ea290SRuslan Ermilov  *    notice, this list of conditions and the following disclaimer.
11407ea290SRuslan Ermilov  * 2. Redistributions in binary form must reproduce the above copyright
12407ea290SRuslan Ermilov  *    notice, this list of conditions and the following disclaimer in the
13407ea290SRuslan Ermilov  *    documentation and/or other materials provided with the distribution.
14407ea290SRuslan Ermilov  *
15407ea290SRuslan Ermilov  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16407ea290SRuslan Ermilov  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17407ea290SRuslan Ermilov  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18407ea290SRuslan Ermilov  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19407ea290SRuslan Ermilov  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20407ea290SRuslan Ermilov  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21407ea290SRuslan Ermilov  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22407ea290SRuslan Ermilov  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23407ea290SRuslan Ermilov  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24407ea290SRuslan Ermilov  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25407ea290SRuslan Ermilov  * SUCH DAMAGE.
26407ea290SRuslan Ermilov  *
27407ea290SRuslan Ermilov  * Author: Ruslan Ermilov <ru@FreeBSD.org>
28407ea290SRuslan Ermilov  *
29407ea290SRuslan Ermilov  * $FreeBSD$
30407ea290SRuslan Ermilov  */
31407ea290SRuslan Ermilov 
32407ea290SRuslan Ermilov #include <sys/param.h>
33407ea290SRuslan Ermilov #include <sys/errno.h>
34407ea290SRuslan Ermilov #include <sys/kernel.h>
35407ea290SRuslan Ermilov #include <sys/malloc.h>
36407ea290SRuslan Ermilov #include <sys/mbuf.h>
37407ea290SRuslan Ermilov #include <sys/queue.h>
38407ea290SRuslan Ermilov #include <sys/socket.h>
39407ea290SRuslan Ermilov #include <sys/systm.h>
40407ea290SRuslan Ermilov 
41407ea290SRuslan Ermilov #include <net/ethernet.h>
42407ea290SRuslan Ermilov #include <net/if.h>
43407ea290SRuslan Ermilov #include <net/if_vlan_var.h>
44407ea290SRuslan Ermilov 
45407ea290SRuslan Ermilov #include <netgraph/ng_message.h>
46407ea290SRuslan Ermilov #include <netgraph/ng_parse.h>
47407ea290SRuslan Ermilov #include <netgraph/ng_vlan.h>
48407ea290SRuslan Ermilov #include <netgraph/netgraph.h>
49407ea290SRuslan Ermilov 
50*bbf53c35SAdrian Chadd struct ng_vlan_private {
51*bbf53c35SAdrian Chadd 	hook_p		downstream_hook;
52*bbf53c35SAdrian Chadd 	hook_p		nomatch_hook;
53*bbf53c35SAdrian Chadd 	uint32_t	decap_enable;
54*bbf53c35SAdrian Chadd 	uint32_t	encap_enable;
55*bbf53c35SAdrian Chadd 	uint16_t	encap_proto;
56*bbf53c35SAdrian Chadd 	hook_p		vlan_hook[(EVL_VLID_MASK + 1)];
57*bbf53c35SAdrian Chadd };
58*bbf53c35SAdrian Chadd typedef struct ng_vlan_private *priv_p;
59*bbf53c35SAdrian Chadd 
60*bbf53c35SAdrian Chadd #define	ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)
61*bbf53c35SAdrian Chadd #define	VLAN_TAG_MASK	0xFFFF
62*bbf53c35SAdrian Chadd #define	HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
63*bbf53c35SAdrian Chadd #define	IS_HOOK_VLAN_SET(hdata) \
64*bbf53c35SAdrian Chadd 	    ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) == HOOK_VLAN_TAG_SET_MASK)
65*bbf53c35SAdrian Chadd 
66407ea290SRuslan Ermilov static ng_constructor_t	ng_vlan_constructor;
67407ea290SRuslan Ermilov static ng_rcvmsg_t	ng_vlan_rcvmsg;
68407ea290SRuslan Ermilov static ng_shutdown_t	ng_vlan_shutdown;
69407ea290SRuslan Ermilov static ng_newhook_t	ng_vlan_newhook;
70407ea290SRuslan Ermilov static ng_rcvdata_t	ng_vlan_rcvdata;
71407ea290SRuslan Ermilov static ng_disconnect_t	ng_vlan_disconnect;
72407ea290SRuslan Ermilov 
73407ea290SRuslan Ermilov /* Parse type for struct ng_vlan_filter. */
74407ea290SRuslan Ermilov static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
75407ea290SRuslan Ermilov 	NG_VLAN_FILTER_FIELDS;
76407ea290SRuslan Ermilov static const struct ng_parse_type ng_vlan_filter_type = {
77407ea290SRuslan Ermilov 	&ng_parse_struct_type,
78407ea290SRuslan Ermilov 	&ng_vlan_filter_fields
79407ea290SRuslan Ermilov };
80407ea290SRuslan Ermilov 
81407ea290SRuslan Ermilov static int
82407ea290SRuslan Ermilov ng_vlan_getTableLength(const struct ng_parse_type *type,
83407ea290SRuslan Ermilov     const u_char *start, const u_char *buf)
84407ea290SRuslan Ermilov {
85407ea290SRuslan Ermilov 	const struct ng_vlan_table *const table =
86407ea290SRuslan Ermilov 	    (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
87407ea290SRuslan Ermilov 
88407ea290SRuslan Ermilov 	return table->n;
89407ea290SRuslan Ermilov }
90407ea290SRuslan Ermilov 
91407ea290SRuslan Ermilov /* Parse type for struct ng_vlan_table. */
92407ea290SRuslan Ermilov static const struct ng_parse_array_info ng_vlan_table_array_info = {
93407ea290SRuslan Ermilov 	&ng_vlan_filter_type,
94407ea290SRuslan Ermilov 	ng_vlan_getTableLength
95407ea290SRuslan Ermilov };
96407ea290SRuslan Ermilov static const struct ng_parse_type ng_vlan_table_array_type = {
97407ea290SRuslan Ermilov 	&ng_parse_array_type,
98407ea290SRuslan Ermilov 	&ng_vlan_table_array_info
99407ea290SRuslan Ermilov };
100407ea290SRuslan Ermilov static const struct ng_parse_struct_field ng_vlan_table_fields[] =
101407ea290SRuslan Ermilov 	NG_VLAN_TABLE_FIELDS;
102407ea290SRuslan Ermilov static const struct ng_parse_type ng_vlan_table_type = {
103407ea290SRuslan Ermilov 	&ng_parse_struct_type,
104407ea290SRuslan Ermilov 	&ng_vlan_table_fields
105407ea290SRuslan Ermilov };
106407ea290SRuslan Ermilov 
107407ea290SRuslan Ermilov /* List of commands and how to convert arguments to/from ASCII. */
108407ea290SRuslan Ermilov static const struct ng_cmdlist ng_vlan_cmdlist[] = {
109407ea290SRuslan Ermilov 	{
110407ea290SRuslan Ermilov 	  NGM_VLAN_COOKIE,
111407ea290SRuslan Ermilov 	  NGM_VLAN_ADD_FILTER,
112407ea290SRuslan Ermilov 	  "addfilter",
113407ea290SRuslan Ermilov 	  &ng_vlan_filter_type,
114407ea290SRuslan Ermilov 	  NULL
115407ea290SRuslan Ermilov 	},
116407ea290SRuslan Ermilov 	{
117407ea290SRuslan Ermilov 	  NGM_VLAN_COOKIE,
118407ea290SRuslan Ermilov 	  NGM_VLAN_DEL_FILTER,
119407ea290SRuslan Ermilov 	  "delfilter",
120407ea290SRuslan Ermilov 	  &ng_parse_hookbuf_type,
121407ea290SRuslan Ermilov 	  NULL
122407ea290SRuslan Ermilov 	},
123407ea290SRuslan Ermilov 	{
124407ea290SRuslan Ermilov 	  NGM_VLAN_COOKIE,
125407ea290SRuslan Ermilov 	  NGM_VLAN_GET_TABLE,
126407ea290SRuslan Ermilov 	  "gettable",
127407ea290SRuslan Ermilov 	  NULL,
128407ea290SRuslan Ermilov 	  &ng_vlan_table_type
129407ea290SRuslan Ermilov 	},
130*bbf53c35SAdrian Chadd 	{
131*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
132*bbf53c35SAdrian Chadd 	  NGM_VLAN_DEL_VID_FLT,
133*bbf53c35SAdrian Chadd 	  "delvidflt",
134*bbf53c35SAdrian Chadd 	  &ng_parse_uint16_type,
135*bbf53c35SAdrian Chadd 	  NULL
136*bbf53c35SAdrian Chadd 	},
137*bbf53c35SAdrian Chadd 	{
138*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
139*bbf53c35SAdrian Chadd 	  NGM_VLAN_GET_DECAP,
140*bbf53c35SAdrian Chadd 	  "getdecap",
141*bbf53c35SAdrian Chadd 	  NULL,
142*bbf53c35SAdrian Chadd 	  &ng_parse_hint32_type
143*bbf53c35SAdrian Chadd 	},
144*bbf53c35SAdrian Chadd 	{
145*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
146*bbf53c35SAdrian Chadd 	  NGM_VLAN_SET_DECAP,
147*bbf53c35SAdrian Chadd 	  "setdecap",
148*bbf53c35SAdrian Chadd 	  &ng_parse_hint32_type,
149*bbf53c35SAdrian Chadd 	  NULL
150*bbf53c35SAdrian Chadd 	},
151*bbf53c35SAdrian Chadd 	{
152*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
153*bbf53c35SAdrian Chadd 	  NGM_VLAN_GET_ENCAP,
154*bbf53c35SAdrian Chadd 	  "getencap",
155*bbf53c35SAdrian Chadd 	  NULL,
156*bbf53c35SAdrian Chadd 	  &ng_parse_hint32_type
157*bbf53c35SAdrian Chadd 	},
158*bbf53c35SAdrian Chadd 	{
159*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
160*bbf53c35SAdrian Chadd 	  NGM_VLAN_SET_ENCAP,
161*bbf53c35SAdrian Chadd 	  "setencap",
162*bbf53c35SAdrian Chadd 	  &ng_parse_hint32_type,
163*bbf53c35SAdrian Chadd 	  NULL
164*bbf53c35SAdrian Chadd 	},
165*bbf53c35SAdrian Chadd 	{
166*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
167*bbf53c35SAdrian Chadd 	  NGM_VLAN_GET_ENCAP_PROTO,
168*bbf53c35SAdrian Chadd 	  "getencapproto",
169*bbf53c35SAdrian Chadd 	  NULL,
170*bbf53c35SAdrian Chadd 	  &ng_parse_hint16_type
171*bbf53c35SAdrian Chadd 	},
172*bbf53c35SAdrian Chadd 	{
173*bbf53c35SAdrian Chadd 	  NGM_VLAN_COOKIE,
174*bbf53c35SAdrian Chadd 	  NGM_VLAN_SET_ENCAP_PROTO,
175*bbf53c35SAdrian Chadd 	  "setencapproto",
176*bbf53c35SAdrian Chadd 	  &ng_parse_hint16_type,
177*bbf53c35SAdrian Chadd 	  NULL
178*bbf53c35SAdrian Chadd 	},
179407ea290SRuslan Ermilov 	{ 0 }
180407ea290SRuslan Ermilov };
181407ea290SRuslan Ermilov 
182407ea290SRuslan Ermilov static struct ng_type ng_vlan_typestruct = {
183f8aae777SJulian Elischer 	.version =	NG_ABI_VERSION,
184f8aae777SJulian Elischer 	.name =		NG_VLAN_NODE_TYPE,
185f8aae777SJulian Elischer 	.constructor =	ng_vlan_constructor,
186f8aae777SJulian Elischer 	.rcvmsg =	ng_vlan_rcvmsg,
187f8aae777SJulian Elischer 	.shutdown =	ng_vlan_shutdown,
188f8aae777SJulian Elischer 	.newhook =	ng_vlan_newhook,
189f8aae777SJulian Elischer 	.rcvdata =	ng_vlan_rcvdata,
190f8aae777SJulian Elischer 	.disconnect =	ng_vlan_disconnect,
191f8aae777SJulian Elischer 	.cmdlist =	ng_vlan_cmdlist,
192407ea290SRuslan Ermilov };
193407ea290SRuslan Ermilov NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
194407ea290SRuslan Ermilov 
195407ea290SRuslan Ermilov 
196*bbf53c35SAdrian Chadd /*
197*bbf53c35SAdrian Chadd  * Helper functions.
198*bbf53c35SAdrian Chadd  */
199407ea290SRuslan Ermilov 
200*bbf53c35SAdrian Chadd static __inline int
201*bbf53c35SAdrian Chadd m_chk(struct mbuf **mp, int len)
202407ea290SRuslan Ermilov {
203407ea290SRuslan Ermilov 
204*bbf53c35SAdrian Chadd 	if ((*mp)->m_pkthdr.len < len) {
205*bbf53c35SAdrian Chadd 		m_freem((*mp));
206*bbf53c35SAdrian Chadd 		(*mp) = NULL;
207*bbf53c35SAdrian Chadd 		return (EINVAL);
208407ea290SRuslan Ermilov 	}
209*bbf53c35SAdrian Chadd 	if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
210*bbf53c35SAdrian Chadd 		return (ENOBUFS);
211*bbf53c35SAdrian Chadd 
212*bbf53c35SAdrian Chadd 	return (0);
213*bbf53c35SAdrian Chadd }
214*bbf53c35SAdrian Chadd 
215*bbf53c35SAdrian Chadd 
216*bbf53c35SAdrian Chadd /*
217*bbf53c35SAdrian Chadd  * Netgraph node functions.
218*bbf53c35SAdrian Chadd  */
219407ea290SRuslan Ermilov 
220407ea290SRuslan Ermilov static int
221407ea290SRuslan Ermilov ng_vlan_constructor(node_p node)
222407ea290SRuslan Ermilov {
223407ea290SRuslan Ermilov 	priv_p priv;
224407ea290SRuslan Ermilov 
225674d86bfSGleb Smirnoff 	priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
226*bbf53c35SAdrian Chadd 	priv->decap_enable = 0;
227*bbf53c35SAdrian Chadd 	priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
228*bbf53c35SAdrian Chadd 	priv->encap_proto = htons(ETHERTYPE_VLAN);
229407ea290SRuslan Ermilov 	NG_NODE_SET_PRIVATE(node, priv);
230407ea290SRuslan Ermilov 	return (0);
231407ea290SRuslan Ermilov }
232407ea290SRuslan Ermilov 
233407ea290SRuslan Ermilov static int
234407ea290SRuslan Ermilov ng_vlan_newhook(node_p node, hook_p hook, const char *name)
235407ea290SRuslan Ermilov {
236407ea290SRuslan Ermilov 	const priv_p priv = NG_NODE_PRIVATE(node);
237407ea290SRuslan Ermilov 
238407ea290SRuslan Ermilov 	if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
239407ea290SRuslan Ermilov 		priv->downstream_hook = hook;
240407ea290SRuslan Ermilov 	else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
241407ea290SRuslan Ermilov 		priv->nomatch_hook = hook;
242407ea290SRuslan Ermilov 	else {
243407ea290SRuslan Ermilov 		/*
244407ea290SRuslan Ermilov 		 * Any other hook name is valid and can
245407ea290SRuslan Ermilov 		 * later be associated with a filter rule.
246407ea290SRuslan Ermilov 		 */
247407ea290SRuslan Ermilov 	}
248407ea290SRuslan Ermilov 	NG_HOOK_SET_PRIVATE(hook, NULL);
249407ea290SRuslan Ermilov 	return (0);
250407ea290SRuslan Ermilov }
251407ea290SRuslan Ermilov 
252407ea290SRuslan Ermilov static int
253407ea290SRuslan Ermilov ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
254407ea290SRuslan Ermilov {
255407ea290SRuslan Ermilov 	const priv_p priv = NG_NODE_PRIVATE(node);
256407ea290SRuslan Ermilov 	struct ng_mesg *msg, *resp = NULL;
257407ea290SRuslan Ermilov 	struct ng_vlan_filter *vf;
258407ea290SRuslan Ermilov 	hook_p hook;
259407ea290SRuslan Ermilov 	struct ng_vlan_table *t;
260*bbf53c35SAdrian Chadd 	uintptr_t hook_data;
261*bbf53c35SAdrian Chadd 	int i, vlan_count;
262*bbf53c35SAdrian Chadd 	uint16_t vid;
263*bbf53c35SAdrian Chadd 	int error = 0;
264407ea290SRuslan Ermilov 
265407ea290SRuslan Ermilov 	NGI_GET_MSG(item, msg);
266407ea290SRuslan Ermilov 	/* Deal with message according to cookie and command. */
267407ea290SRuslan Ermilov 	switch (msg->header.typecookie) {
268407ea290SRuslan Ermilov 	case NGM_VLAN_COOKIE:
269407ea290SRuslan Ermilov 		switch (msg->header.cmd) {
270407ea290SRuslan Ermilov 		case NGM_VLAN_ADD_FILTER:
271407ea290SRuslan Ermilov 			/* Check that message is long enough. */
272407ea290SRuslan Ermilov 			if (msg->header.arglen != sizeof(*vf)) {
273407ea290SRuslan Ermilov 				error = EINVAL;
274407ea290SRuslan Ermilov 				break;
275407ea290SRuslan Ermilov 			}
276407ea290SRuslan Ermilov 			vf = (struct ng_vlan_filter *)msg->data;
277407ea290SRuslan Ermilov 			/* Sanity check the VLAN ID value. */
278*bbf53c35SAdrian Chadd #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
279*bbf53c35SAdrian Chadd 			if (vf->vid == 0 && vf->vid != vf->vlan) {
280*bbf53c35SAdrian Chadd 				vf->vid = vf->vlan;
281*bbf53c35SAdrian Chadd 			} else if (vf->vid != 0 && vf->vlan != 0 &&
282*bbf53c35SAdrian Chadd 			    vf->vid != vf->vlan) {
283*bbf53c35SAdrian Chadd 				error = EINVAL;
284*bbf53c35SAdrian Chadd 				break;
285*bbf53c35SAdrian Chadd 			}
286*bbf53c35SAdrian Chadd #endif
287*bbf53c35SAdrian Chadd 			if (vf->vid & ~EVL_VLID_MASK ||
288*bbf53c35SAdrian Chadd 			    vf->pcp & ~7 ||
289*bbf53c35SAdrian Chadd 			    vf->cfi & ~1) {
290407ea290SRuslan Ermilov 				error = EINVAL;
291407ea290SRuslan Ermilov 				break;
292407ea290SRuslan Ermilov 			}
293407ea290SRuslan Ermilov 			/* Check that a referenced hook exists. */
294*bbf53c35SAdrian Chadd 			hook = ng_findhook(node, vf->hook_name);
295407ea290SRuslan Ermilov 			if (hook == NULL) {
296407ea290SRuslan Ermilov 				error = ENOENT;
297407ea290SRuslan Ermilov 				break;
298407ea290SRuslan Ermilov 			}
299407ea290SRuslan Ermilov 			/* And is not one of the special hooks. */
300407ea290SRuslan Ermilov 			if (hook == priv->downstream_hook ||
301407ea290SRuslan Ermilov 			    hook == priv->nomatch_hook) {
302407ea290SRuslan Ermilov 				error = EINVAL;
303407ea290SRuslan Ermilov 				break;
304407ea290SRuslan Ermilov 			}
305407ea290SRuslan Ermilov 			/* And is not already in service. */
306*bbf53c35SAdrian Chadd 			if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
307407ea290SRuslan Ermilov 				error = EEXIST;
308407ea290SRuslan Ermilov 				break;
309407ea290SRuslan Ermilov 			}
310407ea290SRuslan Ermilov 			/* Check we don't already trap this VLAN. */
311*bbf53c35SAdrian Chadd 			if (priv->vlan_hook[vf->vid] != NULL) {
312407ea290SRuslan Ermilov 				error = EEXIST;
313407ea290SRuslan Ermilov 				break;
314407ea290SRuslan Ermilov 			}
315*bbf53c35SAdrian Chadd 			/* Link vlan and hook together. */
316*bbf53c35SAdrian Chadd 			NG_HOOK_SET_PRIVATE(hook,
317*bbf53c35SAdrian Chadd 			    (void *)(HOOK_VLAN_TAG_SET_MASK |
318*bbf53c35SAdrian Chadd 			    EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
319*bbf53c35SAdrian Chadd 			priv->vlan_hook[vf->vid] = hook;
320407ea290SRuslan Ermilov 			break;
321407ea290SRuslan Ermilov 		case NGM_VLAN_DEL_FILTER:
322407ea290SRuslan Ermilov 			/* Check that message is long enough. */
323407ea290SRuslan Ermilov 			if (msg->header.arglen != NG_HOOKSIZ) {
324407ea290SRuslan Ermilov 				error = EINVAL;
325407ea290SRuslan Ermilov 				break;
326407ea290SRuslan Ermilov 			}
327407ea290SRuslan Ermilov 			/* Check that hook exists and is active. */
328407ea290SRuslan Ermilov 			hook = ng_findhook(node, (char *)msg->data);
329*bbf53c35SAdrian Chadd 			if (hook == NULL) {
330407ea290SRuslan Ermilov 				error = ENOENT;
331407ea290SRuslan Ermilov 				break;
332407ea290SRuslan Ermilov 			}
333*bbf53c35SAdrian Chadd 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
334*bbf53c35SAdrian Chadd 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
335*bbf53c35SAdrian Chadd 				error = ENOENT;
336*bbf53c35SAdrian Chadd 				break;
337*bbf53c35SAdrian Chadd 			}
338*bbf53c35SAdrian Chadd 
339*bbf53c35SAdrian Chadd 			KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
340*bbf53c35SAdrian Chadd 			    ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
341*bbf53c35SAdrian Chadd 			    __func__, (char *)msg->data));
342*bbf53c35SAdrian Chadd 
343407ea290SRuslan Ermilov 			/* Purge a rule that refers to this hook. */
344*bbf53c35SAdrian Chadd 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
345407ea290SRuslan Ermilov 			NG_HOOK_SET_PRIVATE(hook, NULL);
346*bbf53c35SAdrian Chadd 			break;
347*bbf53c35SAdrian Chadd 		case NGM_VLAN_DEL_VID_FLT:
348*bbf53c35SAdrian Chadd 			/* Check that message is long enough. */
349*bbf53c35SAdrian Chadd 			if (msg->header.arglen != sizeof(uint16_t)) {
350*bbf53c35SAdrian Chadd 				error = EINVAL;
351*bbf53c35SAdrian Chadd 				break;
352*bbf53c35SAdrian Chadd 			}
353*bbf53c35SAdrian Chadd 			vid = (*((uint16_t *)msg->data));
354*bbf53c35SAdrian Chadd 			/* Sanity check the VLAN ID value. */
355*bbf53c35SAdrian Chadd 			if (vid & ~EVL_VLID_MASK) {
356*bbf53c35SAdrian Chadd 				error = EINVAL;
357*bbf53c35SAdrian Chadd 				break;
358*bbf53c35SAdrian Chadd 			}
359*bbf53c35SAdrian Chadd 			/* Check that hook exists and is active. */
360*bbf53c35SAdrian Chadd 			hook = priv->vlan_hook[vid];
361*bbf53c35SAdrian Chadd 			if (hook == NULL) {
362*bbf53c35SAdrian Chadd 				error = ENOENT;
363*bbf53c35SAdrian Chadd 				break;
364*bbf53c35SAdrian Chadd 			}
365*bbf53c35SAdrian Chadd 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
366*bbf53c35SAdrian Chadd 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
367*bbf53c35SAdrian Chadd 				error = ENOENT;
368*bbf53c35SAdrian Chadd 				break;
369*bbf53c35SAdrian Chadd 			}
370*bbf53c35SAdrian Chadd 
371*bbf53c35SAdrian Chadd 			KASSERT(EVL_VLANOFTAG(hook_data) == vid,
372*bbf53c35SAdrian Chadd 			    ("%s: NGM_VLAN_DEL_VID_FLT:"
373*bbf53c35SAdrian Chadd 			    " Invalid VID Hook = %us, must be: %us\n",
374*bbf53c35SAdrian Chadd 			    __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
375*bbf53c35SAdrian Chadd 			    vid));
376*bbf53c35SAdrian Chadd 
377*bbf53c35SAdrian Chadd 			/* Purge a rule that refers to this hook. */
378*bbf53c35SAdrian Chadd 			priv->vlan_hook[vid] = NULL;
379*bbf53c35SAdrian Chadd 			NG_HOOK_SET_PRIVATE(hook, NULL);
380407ea290SRuslan Ermilov 			break;
381407ea290SRuslan Ermilov 		case NGM_VLAN_GET_TABLE:
382*bbf53c35SAdrian Chadd 			/* Calculate vlans. */
383*bbf53c35SAdrian Chadd 			vlan_count = 0;
384*bbf53c35SAdrian Chadd 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
385*bbf53c35SAdrian Chadd 				if (priv->vlan_hook[i] != NULL &&
386*bbf53c35SAdrian Chadd 				    NG_HOOK_IS_VALID(priv->vlan_hook[i]))
387*bbf53c35SAdrian Chadd 					vlan_count ++;
388*bbf53c35SAdrian Chadd 			}
389*bbf53c35SAdrian Chadd 
390*bbf53c35SAdrian Chadd 			/* Allocate memory for responce. */
391407ea290SRuslan Ermilov 			NG_MKRESPONSE(resp, msg, sizeof(*t) +
392*bbf53c35SAdrian Chadd 			    vlan_count * sizeof(*t->filter), M_NOWAIT);
393407ea290SRuslan Ermilov 			if (resp == NULL) {
394407ea290SRuslan Ermilov 				error = ENOMEM;
395407ea290SRuslan Ermilov 				break;
396407ea290SRuslan Ermilov 			}
397*bbf53c35SAdrian Chadd 
398*bbf53c35SAdrian Chadd 			/* Pack data to responce. */
399407ea290SRuslan Ermilov 			t = (struct ng_vlan_table *)resp->data;
400*bbf53c35SAdrian Chadd 			t->n = 0;
401407ea290SRuslan Ermilov 			vf = &t->filter[0];
402*bbf53c35SAdrian Chadd 			for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
403*bbf53c35SAdrian Chadd 				hook = priv->vlan_hook[i];
404*bbf53c35SAdrian Chadd 				if (hook == NULL || NG_HOOK_NOT_VALID(hook))
405*bbf53c35SAdrian Chadd 					continue;
406*bbf53c35SAdrian Chadd 				hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
407*bbf53c35SAdrian Chadd 				if (IS_HOOK_VLAN_SET(hook_data) == 0)
408*bbf53c35SAdrian Chadd 					continue;
409*bbf53c35SAdrian Chadd 
410*bbf53c35SAdrian Chadd 				KASSERT(EVL_VLANOFTAG(hook_data) == i,
411*bbf53c35SAdrian Chadd 				    ("%s: NGM_VLAN_GET_TABLE:"
412*bbf53c35SAdrian Chadd 				    " hook %s VID = %us, must be: %i\n",
413*bbf53c35SAdrian Chadd 				    __func__, NG_HOOK_NAME(hook),
414*bbf53c35SAdrian Chadd 				    (uint16_t)EVL_VLANOFTAG(hook_data), i));
415*bbf53c35SAdrian Chadd 
416*bbf53c35SAdrian Chadd #ifdef	NG_VLAN_USE_OLD_VLAN_NAME
417*bbf53c35SAdrian Chadd 				vf->vlan = i;
418*bbf53c35SAdrian Chadd #endif
419*bbf53c35SAdrian Chadd 				vf->vid = i;
420*bbf53c35SAdrian Chadd 				vf->pcp = EVL_PRIOFTAG(hook_data);
421*bbf53c35SAdrian Chadd 				vf->cfi = EVL_CFIOFTAG(hook_data);
422*bbf53c35SAdrian Chadd 				strncpy(vf->hook_name,
423*bbf53c35SAdrian Chadd 				    NG_HOOK_NAME(hook), NG_HOOKSIZ);
424407ea290SRuslan Ermilov 				vf ++;
425*bbf53c35SAdrian Chadd 				t->n ++;
426407ea290SRuslan Ermilov 			}
427*bbf53c35SAdrian Chadd 			break;
428*bbf53c35SAdrian Chadd 		case NGM_VLAN_GET_DECAP:
429*bbf53c35SAdrian Chadd 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
430*bbf53c35SAdrian Chadd 			if (resp == NULL) {
431*bbf53c35SAdrian Chadd 				error = ENOMEM;
432*bbf53c35SAdrian Chadd 				break;
433407ea290SRuslan Ermilov 			}
434*bbf53c35SAdrian Chadd 			(*((uint32_t *)resp->data)) = priv->decap_enable;
435*bbf53c35SAdrian Chadd 			break;
436*bbf53c35SAdrian Chadd 		case NGM_VLAN_SET_DECAP:
437*bbf53c35SAdrian Chadd 			if (msg->header.arglen != sizeof(uint32_t)) {
438*bbf53c35SAdrian Chadd 				error = EINVAL;
439*bbf53c35SAdrian Chadd 				break;
440*bbf53c35SAdrian Chadd 			}
441*bbf53c35SAdrian Chadd 			priv->decap_enable = (*((uint32_t *)msg->data));
442*bbf53c35SAdrian Chadd 			break;
443*bbf53c35SAdrian Chadd 		case NGM_VLAN_GET_ENCAP:
444*bbf53c35SAdrian Chadd 			NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
445*bbf53c35SAdrian Chadd 			if (resp == NULL) {
446*bbf53c35SAdrian Chadd 				error = ENOMEM;
447*bbf53c35SAdrian Chadd 				break;
448*bbf53c35SAdrian Chadd 			}
449*bbf53c35SAdrian Chadd 			(*((uint32_t *)resp->data)) = priv->encap_enable;
450*bbf53c35SAdrian Chadd 			break;
451*bbf53c35SAdrian Chadd 		case NGM_VLAN_SET_ENCAP:
452*bbf53c35SAdrian Chadd 			if (msg->header.arglen != sizeof(uint32_t)) {
453*bbf53c35SAdrian Chadd 				error = EINVAL;
454*bbf53c35SAdrian Chadd 				break;
455*bbf53c35SAdrian Chadd 			}
456*bbf53c35SAdrian Chadd 			priv->encap_enable = (*((uint32_t *)msg->data));
457*bbf53c35SAdrian Chadd 			break;
458*bbf53c35SAdrian Chadd 		case NGM_VLAN_GET_ENCAP_PROTO:
459*bbf53c35SAdrian Chadd 			NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
460*bbf53c35SAdrian Chadd 			if (resp == NULL) {
461*bbf53c35SAdrian Chadd 				error = ENOMEM;
462*bbf53c35SAdrian Chadd 				break;
463*bbf53c35SAdrian Chadd 			}
464*bbf53c35SAdrian Chadd 			(*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
465*bbf53c35SAdrian Chadd 			break;
466*bbf53c35SAdrian Chadd 		case NGM_VLAN_SET_ENCAP_PROTO:
467*bbf53c35SAdrian Chadd 			if (msg->header.arglen != sizeof(uint16_t)) {
468*bbf53c35SAdrian Chadd 				error = EINVAL;
469*bbf53c35SAdrian Chadd 				break;
470*bbf53c35SAdrian Chadd 			}
471*bbf53c35SAdrian Chadd 			priv->encap_proto = htons((*((uint16_t *)msg->data)));
472407ea290SRuslan Ermilov 			break;
473407ea290SRuslan Ermilov 		default: /* Unknown command. */
474407ea290SRuslan Ermilov 			error = EINVAL;
475407ea290SRuslan Ermilov 			break;
476407ea290SRuslan Ermilov 		}
477407ea290SRuslan Ermilov 		break;
4783b1c41c5SGleb Smirnoff 	case NGM_FLOW_COOKIE:
4793b1c41c5SGleb Smirnoff 	    {
4803b1c41c5SGleb Smirnoff 		struct ng_mesg *copy;
4813b1c41c5SGleb Smirnoff 
4823b1c41c5SGleb Smirnoff 		/*
4833b1c41c5SGleb Smirnoff 		 * Flow control messages should come only
4843b1c41c5SGleb Smirnoff 		 * from downstream.
4853b1c41c5SGleb Smirnoff 		 */
4863b1c41c5SGleb Smirnoff 
4873b1c41c5SGleb Smirnoff 		if (lasthook == NULL)
4883b1c41c5SGleb Smirnoff 			break;
4893b1c41c5SGleb Smirnoff 		if (lasthook != priv->downstream_hook)
4903b1c41c5SGleb Smirnoff 			break;
4913b1c41c5SGleb Smirnoff 		/* Broadcast the event to all uplinks. */
492*bbf53c35SAdrian Chadd 		for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
493*bbf53c35SAdrian Chadd 			if (priv->vlan_hook[i] == NULL)
494*bbf53c35SAdrian Chadd 				continue;
495*bbf53c35SAdrian Chadd 
4963b1c41c5SGleb Smirnoff 			NG_COPYMESSAGE(copy, msg, M_NOWAIT);
4973b1c41c5SGleb Smirnoff 			if (copy == NULL)
4983b1c41c5SGleb Smirnoff 				continue;
499*bbf53c35SAdrian Chadd 			NG_SEND_MSG_HOOK(error, node, copy,
500*bbf53c35SAdrian Chadd 			    priv->vlan_hook[i], 0);
5013b1c41c5SGleb Smirnoff 		}
5023b1c41c5SGleb Smirnoff 		break;
5033b1c41c5SGleb Smirnoff 	    }
504407ea290SRuslan Ermilov 	default: /* Unknown type cookie. */
505407ea290SRuslan Ermilov 		error = EINVAL;
506407ea290SRuslan Ermilov 		break;
507407ea290SRuslan Ermilov 	}
508407ea290SRuslan Ermilov 	NG_RESPOND_MSG(error, node, item, resp);
509407ea290SRuslan Ermilov 	NG_FREE_MSG(msg);
510407ea290SRuslan Ermilov 	return (error);
511407ea290SRuslan Ermilov }
512407ea290SRuslan Ermilov 
513407ea290SRuslan Ermilov static int
514407ea290SRuslan Ermilov ng_vlan_rcvdata(hook_p hook, item_p item)
515407ea290SRuslan Ermilov {
516407ea290SRuslan Ermilov 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
517407ea290SRuslan Ermilov 	struct ether_header *eh;
518*bbf53c35SAdrian Chadd 	struct ether_vlan_header *evl;
519407ea290SRuslan Ermilov 	int error;
520*bbf53c35SAdrian Chadd 	uintptr_t hook_data;
521*bbf53c35SAdrian Chadd 	uint16_t vid, eth_vtag;
522407ea290SRuslan Ermilov 	struct mbuf *m;
523*bbf53c35SAdrian Chadd 	hook_p dst_hook;
524*bbf53c35SAdrian Chadd 
525*bbf53c35SAdrian Chadd 
526*bbf53c35SAdrian Chadd 	NGI_GET_M(item, m);
527407ea290SRuslan Ermilov 
528407ea290SRuslan Ermilov 	/* Make sure we have an entire header. */
529*bbf53c35SAdrian Chadd 	error = m_chk(&m, ETHER_HDR_LEN);
530*bbf53c35SAdrian Chadd 	if (error != 0)
531*bbf53c35SAdrian Chadd 		goto mchk_err;
532*bbf53c35SAdrian Chadd 
533407ea290SRuslan Ermilov 	eh = mtod(m, struct ether_header *);
534407ea290SRuslan Ermilov 	if (hook == priv->downstream_hook) {
535407ea290SRuslan Ermilov 		/*
536407ea290SRuslan Ermilov 		 * If from downstream, select between a match hook
537407ea290SRuslan Ermilov 		 * or the nomatch hook.
538407ea290SRuslan Ermilov 		 */
539*bbf53c35SAdrian Chadd 
540*bbf53c35SAdrian Chadd 		dst_hook = priv->nomatch_hook;
541*bbf53c35SAdrian Chadd 
542*bbf53c35SAdrian Chadd 		/* Skip packets without tag. */
543*bbf53c35SAdrian Chadd 		if ((m->m_flags & M_VLANTAG) == 0 &&
544*bbf53c35SAdrian Chadd 		    eh->ether_type != priv->encap_proto) {
545*bbf53c35SAdrian Chadd 			if (dst_hook == NULL)
546*bbf53c35SAdrian Chadd 				goto net_down;
547*bbf53c35SAdrian Chadd 			goto send_packet;
548*bbf53c35SAdrian Chadd 		}
549*bbf53c35SAdrian Chadd 
550*bbf53c35SAdrian Chadd 		/* Process packets with tag. */
55178ba57b9SAndre Oppermann 		if (m->m_flags & M_VLANTAG) {
552407ea290SRuslan Ermilov 			/*
553407ea290SRuslan Ermilov 			 * Packet is tagged, m contains a normal
554407ea290SRuslan Ermilov 			 * Ethernet frame; tag is stored out-of-band.
555407ea290SRuslan Ermilov 			 */
556*bbf53c35SAdrian Chadd 			evl = NULL;
557*bbf53c35SAdrian Chadd 			vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
558*bbf53c35SAdrian Chadd 		} else { /* eh->ether_type == priv->encap_proto */
559*bbf53c35SAdrian Chadd 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
560*bbf53c35SAdrian Chadd 			if (error != 0)
561*bbf53c35SAdrian Chadd 				goto mchk_err;
562407ea290SRuslan Ermilov 			evl = mtod(m, struct ether_vlan_header *);
563*bbf53c35SAdrian Chadd 			vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
564407ea290SRuslan Ermilov 		}
565*bbf53c35SAdrian Chadd 
566*bbf53c35SAdrian Chadd 		if (priv->vlan_hook[vid] != NULL) {
567*bbf53c35SAdrian Chadd 			/*
568*bbf53c35SAdrian Chadd 			 * VLAN filter: allways remove vlan tags and
569*bbf53c35SAdrian Chadd 			 * decapsulate packet.
570*bbf53c35SAdrian Chadd 			 */
571*bbf53c35SAdrian Chadd 			dst_hook = priv->vlan_hook[vid];
572*bbf53c35SAdrian Chadd 			if (evl == NULL) { /* m->m_flags & M_VLANTAG */
57378ba57b9SAndre Oppermann 				m->m_pkthdr.ether_vtag = 0;
57478ba57b9SAndre Oppermann 				m->m_flags &= ~M_VLANTAG;
575*bbf53c35SAdrian Chadd 				goto send_packet;
576*bbf53c35SAdrian Chadd 			}
577*bbf53c35SAdrian Chadd 		} else { /* nomatch_hook */
578*bbf53c35SAdrian Chadd 			if (dst_hook == NULL)
579*bbf53c35SAdrian Chadd 				goto net_down;
580*bbf53c35SAdrian Chadd 			if (evl == NULL || priv->decap_enable == 0)
581*bbf53c35SAdrian Chadd 				goto send_packet;
582*bbf53c35SAdrian Chadd 			/* Save tag out-of-band. */
583*bbf53c35SAdrian Chadd 			m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
584*bbf53c35SAdrian Chadd 			m->m_flags |= M_VLANTAG;
585*bbf53c35SAdrian Chadd 		}
586*bbf53c35SAdrian Chadd 
587*bbf53c35SAdrian Chadd 		/*
588*bbf53c35SAdrian Chadd 		 * Decapsulate:
589*bbf53c35SAdrian Chadd 		 * TPID = ether type encap
590*bbf53c35SAdrian Chadd 		 * Move DstMAC and SrcMAC to ETHER_TYPE.
591*bbf53c35SAdrian Chadd 		 * Before:
592*bbf53c35SAdrian Chadd 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
593*bbf53c35SAdrian Chadd 		 *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
594*bbf53c35SAdrian Chadd 		 * After:
595*bbf53c35SAdrian Chadd 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
596*bbf53c35SAdrian Chadd 		 *                |-----------| |--------------------|
597*bbf53c35SAdrian Chadd 		 */
598*bbf53c35SAdrian Chadd 		bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
599*bbf53c35SAdrian Chadd 		    (ETHER_ADDR_LEN * 2));
600407ea290SRuslan Ermilov 		m_adj(m, ETHER_VLAN_ENCAP_LEN);
601407ea290SRuslan Ermilov 	} else {
602407ea290SRuslan Ermilov 		/*
603407ea290SRuslan Ermilov 		 * It is heading towards the downstream.
604407ea290SRuslan Ermilov 		 * If from nomatch, pass it unmodified.
605407ea290SRuslan Ermilov 		 * Otherwise, do the VLAN encapsulation.
606407ea290SRuslan Ermilov 		 */
607*bbf53c35SAdrian Chadd 		dst_hook = priv->downstream_hook;
608*bbf53c35SAdrian Chadd 		if (dst_hook == NULL)
609*bbf53c35SAdrian Chadd 			goto net_down;
610*bbf53c35SAdrian Chadd 		if (hook != priv->nomatch_hook) {/* Filter hook. */
611*bbf53c35SAdrian Chadd 			hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
612*bbf53c35SAdrian Chadd 			if (IS_HOOK_VLAN_SET(hook_data) == 0) {
613*bbf53c35SAdrian Chadd 				/*
614*bbf53c35SAdrian Chadd 				 * Packet from hook not in filter
615*bbf53c35SAdrian Chadd 				 * call addfilter for this hook to fix.
616*bbf53c35SAdrian Chadd 				 */
617*bbf53c35SAdrian Chadd 				error = EOPNOTSUPP;
618*bbf53c35SAdrian Chadd 				goto drop;
619407ea290SRuslan Ermilov 			}
620*bbf53c35SAdrian Chadd 			eth_vtag = (hook_data & VLAN_TAG_MASK);
621*bbf53c35SAdrian Chadd 			if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
622*bbf53c35SAdrian Chadd 				/* Just set packet header tag and send. */
623*bbf53c35SAdrian Chadd 				m->m_flags |= M_VLANTAG;
624*bbf53c35SAdrian Chadd 				m->m_pkthdr.ether_vtag = eth_vtag;
625*bbf53c35SAdrian Chadd 				goto send_packet;
626407ea290SRuslan Ermilov 			}
627*bbf53c35SAdrian Chadd 		} else { /* nomatch_hook */
628*bbf53c35SAdrian Chadd 			if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
629*bbf53c35SAdrian Chadd 			    (m->m_flags & M_VLANTAG) == 0)
630*bbf53c35SAdrian Chadd 				goto send_packet;
631*bbf53c35SAdrian Chadd 			/* Encapsulate tagged packet. */
632*bbf53c35SAdrian Chadd 			eth_vtag = m->m_pkthdr.ether_vtag;
633*bbf53c35SAdrian Chadd 			m->m_pkthdr.ether_vtag = 0;
634*bbf53c35SAdrian Chadd 			m->m_flags &= ~M_VLANTAG;
635*bbf53c35SAdrian Chadd 		}
636*bbf53c35SAdrian Chadd 
637407ea290SRuslan Ermilov 		/*
638407ea290SRuslan Ermilov 		 * Transform the Ethernet header into an Ethernet header
639407ea290SRuslan Ermilov 		 * with 802.1Q encapsulation.
640*bbf53c35SAdrian Chadd 		 * Mod of: ether_vlanencap.
641*bbf53c35SAdrian Chadd 		 *
642*bbf53c35SAdrian Chadd 		 * TPID = ether type encap
643*bbf53c35SAdrian Chadd 		 * Move DstMAC and SrcMAC from ETHER_TYPE.
644*bbf53c35SAdrian Chadd 		 * Before:
645*bbf53c35SAdrian Chadd 		 *  [free space ] [dmac] [smac] [ether_type] [payload]
646*bbf53c35SAdrian Chadd 		 *  <<<<<<<<<<<<< |-----------| |--------------------|
647*bbf53c35SAdrian Chadd 		 * After:
648*bbf53c35SAdrian Chadd 		 *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
649*bbf53c35SAdrian Chadd 		 *  |-----------| |-- inserted tag --| |--------------------|
650407ea290SRuslan Ermilov 		 */
651*bbf53c35SAdrian Chadd 		M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
652*bbf53c35SAdrian Chadd 		if (m == NULL)
653*bbf53c35SAdrian Chadd 			error = ENOMEM;
654*bbf53c35SAdrian Chadd 		else
655*bbf53c35SAdrian Chadd 			error = m_chk(&m, ETHER_VLAN_HDR_LEN);
656*bbf53c35SAdrian Chadd 		if (error != 0)
657*bbf53c35SAdrian Chadd 			goto mchk_err;
658*bbf53c35SAdrian Chadd 
659407ea290SRuslan Ermilov 		evl = mtod(m, struct ether_vlan_header *);
660*bbf53c35SAdrian Chadd 		bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
661*bbf53c35SAdrian Chadd 		    (char *)evl, (ETHER_ADDR_LEN * 2));
662*bbf53c35SAdrian Chadd 		evl->evl_encap_proto = priv->encap_proto;
663*bbf53c35SAdrian Chadd 		evl->evl_tag = htons(eth_vtag);
664407ea290SRuslan Ermilov 	}
665*bbf53c35SAdrian Chadd 
666*bbf53c35SAdrian Chadd send_packet:
667*bbf53c35SAdrian Chadd 	NG_FWD_NEW_DATA(error, item, dst_hook, m);
668*bbf53c35SAdrian Chadd 	return (error);
669*bbf53c35SAdrian Chadd net_down:
670*bbf53c35SAdrian Chadd 	error = ENETDOWN;
671*bbf53c35SAdrian Chadd drop:
672*bbf53c35SAdrian Chadd 	m_freem(m);
673*bbf53c35SAdrian Chadd mchk_err:
674*bbf53c35SAdrian Chadd 	NG_FREE_ITEM(item);
675407ea290SRuslan Ermilov 	return (error);
676407ea290SRuslan Ermilov }
677407ea290SRuslan Ermilov 
678407ea290SRuslan Ermilov static int
679407ea290SRuslan Ermilov ng_vlan_shutdown(node_p node)
680407ea290SRuslan Ermilov {
681407ea290SRuslan Ermilov 	const priv_p priv = NG_NODE_PRIVATE(node);
682407ea290SRuslan Ermilov 
683407ea290SRuslan Ermilov 	NG_NODE_SET_PRIVATE(node, NULL);
684407ea290SRuslan Ermilov 	NG_NODE_UNREF(node);
6851ede983cSDag-Erling Smørgrav 	free(priv, M_NETGRAPH);
686407ea290SRuslan Ermilov 	return (0);
687407ea290SRuslan Ermilov }
688407ea290SRuslan Ermilov 
689407ea290SRuslan Ermilov static int
690407ea290SRuslan Ermilov ng_vlan_disconnect(hook_p hook)
691407ea290SRuslan Ermilov {
692407ea290SRuslan Ermilov 	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
693*bbf53c35SAdrian Chadd 	uintptr_t hook_data;
694407ea290SRuslan Ermilov 
695407ea290SRuslan Ermilov 	if (hook == priv->downstream_hook)
696407ea290SRuslan Ermilov 		priv->downstream_hook = NULL;
697407ea290SRuslan Ermilov 	else if (hook == priv->nomatch_hook)
698407ea290SRuslan Ermilov 		priv->nomatch_hook = NULL;
699407ea290SRuslan Ermilov 	else {
700407ea290SRuslan Ermilov 		/* Purge a rule that refers to this hook. */
701*bbf53c35SAdrian Chadd 		hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
702*bbf53c35SAdrian Chadd 		if (IS_HOOK_VLAN_SET(hook_data))
703*bbf53c35SAdrian Chadd 			priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
704407ea290SRuslan Ermilov 	}
705407ea290SRuslan Ermilov 	NG_HOOK_SET_PRIVATE(hook, NULL);
706407ea290SRuslan Ermilov 	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
707407ea290SRuslan Ermilov 	    (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
708407ea290SRuslan Ermilov 		ng_rmnode_self(NG_HOOK_NODE(hook));
709407ea290SRuslan Ermilov 	return (0);
710407ea290SRuslan Ermilov }
711