1/*-
2 * Copyright (c) 2003 IPNET Internet Communication Company
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * Author: Ruslan Ermilov <ru@FreeBSD.org>
27 *
28 * $FreeBSD$
29 */
30
31#include <sys/param.h>
32#include <sys/errno.h>
33#include <sys/kernel.h>
34#include <sys/malloc.h>
35#include <sys/mbuf.h>
36#include <sys/queue.h>
37#include <sys/socket.h>
38#include <sys/systm.h>
39
40#include <net/ethernet.h>
41#include <net/if.h>
42#include <net/if_vlan_var.h>
43
44#include <netgraph/ng_message.h>
45#include <netgraph/ng_parse.h>
46#include <netgraph/ng_vlan.h>
47#include <netgraph/netgraph.h>
48
49static ng_constructor_t	ng_vlan_constructor;
50static ng_rcvmsg_t	ng_vlan_rcvmsg;
51static ng_shutdown_t	ng_vlan_shutdown;
52static ng_newhook_t	ng_vlan_newhook;
53static ng_rcvdata_t	ng_vlan_rcvdata;
54static ng_disconnect_t	ng_vlan_disconnect;
55
56/* Parse type for struct ng_vlan_filter. */
57static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
58	NG_VLAN_FILTER_FIELDS;
59static const struct ng_parse_type ng_vlan_filter_type = {
60	&ng_parse_struct_type,
61	&ng_vlan_filter_fields
62};
63
64static int
65ng_vlan_getTableLength(const struct ng_parse_type *type,
66    const u_char *start, const u_char *buf)
67{
68	const struct ng_vlan_table *const table =
69	    (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
70
71	return table->n;
72}
73
74/* Parse type for struct ng_vlan_table. */
75static const struct ng_parse_array_info ng_vlan_table_array_info = {
76	&ng_vlan_filter_type,
77	ng_vlan_getTableLength
78};
79static const struct ng_parse_type ng_vlan_table_array_type = {
80	&ng_parse_array_type,
81	&ng_vlan_table_array_info
82};
83static const struct ng_parse_struct_field ng_vlan_table_fields[] =
84	NG_VLAN_TABLE_FIELDS;
85static const struct ng_parse_type ng_vlan_table_type = {
86	&ng_parse_struct_type,
87	&ng_vlan_table_fields
88};
89
90/* List of commands and how to convert arguments to/from ASCII. */
91static const struct ng_cmdlist ng_vlan_cmdlist[] = {
92	{
93	  NGM_VLAN_COOKIE,
94	  NGM_VLAN_ADD_FILTER,
95	  "addfilter",
96	  &ng_vlan_filter_type,
97	  NULL
98	},
99	{
100	  NGM_VLAN_COOKIE,
101	  NGM_VLAN_DEL_FILTER,
102	  "delfilter",
103	  &ng_parse_hookbuf_type,
104	  NULL
105	},
106	{
107	  NGM_VLAN_COOKIE,
108	  NGM_VLAN_GET_TABLE,
109	  "gettable",
110	  NULL,
111	  &ng_vlan_table_type
112	},
113	{ 0 }
114};
115
116static struct ng_type ng_vlan_typestruct = {
117	.version =	NG_ABI_VERSION,
118	.name =		NG_VLAN_NODE_TYPE,
119	.constructor =	ng_vlan_constructor,
120	.rcvmsg =	ng_vlan_rcvmsg,
121	.shutdown =	ng_vlan_shutdown,
122	.newhook =	ng_vlan_newhook,
123	.rcvdata =	ng_vlan_rcvdata,
124	.disconnect =	ng_vlan_disconnect,
125	.cmdlist =	ng_vlan_cmdlist,
126};
127NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
128
129struct filter {
130	LIST_ENTRY(filter) next;
131	u_int16_t	vlan;
132	hook_p		hook;
133};
134
135#define	HASHSIZE	16
136#define	HASH(id)	((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f)
137LIST_HEAD(filterhead, filter);
138
139typedef struct {
140	hook_p		downstream_hook;
141	hook_p		nomatch_hook;
142	struct filterhead hashtable[HASHSIZE];
143	u_int32_t	nent;
144} *priv_p;
145
146static struct filter *
147ng_vlan_findentry(priv_p priv, u_int16_t vlan)
148{
149	struct filterhead *chain = &priv->hashtable[HASH(vlan)];
150	struct filter *f;
151
152	LIST_FOREACH(f, chain, next)
153		if (f->vlan == vlan)
154			return (f);
155	return (NULL);
156}
157
158static int
159ng_vlan_constructor(node_p node)
160{
161	priv_p priv;
162	int i;
163
164	priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
165	for (i = 0; i < HASHSIZE; i++)
166		LIST_INIT(&priv->hashtable[i]);
167	NG_NODE_SET_PRIVATE(node, priv);
168	return (0);
169}
170
171static int
172ng_vlan_newhook(node_p node, hook_p hook, const char *name)
173{
174	const priv_p priv = NG_NODE_PRIVATE(node);
175
176	if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
177		priv->downstream_hook = hook;
178	else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
179		priv->nomatch_hook = hook;
180	else {
181		/*
182		 * Any other hook name is valid and can
183		 * later be associated with a filter rule.
184		 */
185	}
186	NG_HOOK_SET_PRIVATE(hook, NULL);
187	return (0);
188}
189
190static int
191ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
192{
193	const priv_p priv = NG_NODE_PRIVATE(node);
194	int error = 0;
195	struct ng_mesg *msg, *resp = NULL;
196	struct ng_vlan_filter *vf;
197	struct filter *f;
198	hook_p hook;
199	struct ng_vlan_table *t;
200	int i;
201
202	NGI_GET_MSG(item, msg);
203	/* Deal with message according to cookie and command. */
204	switch (msg->header.typecookie) {
205	case NGM_VLAN_COOKIE:
206		switch (msg->header.cmd) {
207		case NGM_VLAN_ADD_FILTER:
208			/* Check that message is long enough. */
209			if (msg->header.arglen != sizeof(*vf)) {
210				error = EINVAL;
211				break;
212			}
213			vf = (struct ng_vlan_filter *)msg->data;
214			/* Sanity check the VLAN ID value. */
215			if (vf->vlan & ~EVL_VLID_MASK) {
216				error = EINVAL;
217				break;
218			}
219			/* Check that a referenced hook exists. */
220			hook = ng_findhook(node, vf->hook);
221			if (hook == NULL) {
222				error = ENOENT;
223				break;
224			}
225			/* And is not one of the special hooks. */
226			if (hook == priv->downstream_hook ||
227			    hook == priv->nomatch_hook) {
228				error = EINVAL;
229				break;
230			}
231			/* And is not already in service. */
232			if (NG_HOOK_PRIVATE(hook) != NULL) {
233				error = EEXIST;
234				break;
235			}
236			/* Check we don't already trap this VLAN. */
237			if (ng_vlan_findentry(priv, vf->vlan)) {
238				error = EEXIST;
239				break;
240			}
241			/* Create filter. */
242			f = malloc(sizeof(*f),
243			    M_NETGRAPH, M_NOWAIT | M_ZERO);
244			if (f == NULL) {
245				error = ENOMEM;
246				break;
247			}
248			/* Link filter and hook together. */
249			f->hook = hook;
250			f->vlan = vf->vlan;
251			NG_HOOK_SET_PRIVATE(hook, f);
252			/* Register filter in a hash table. */
253			LIST_INSERT_HEAD(
254			    &priv->hashtable[HASH(f->vlan)], f, next);
255			priv->nent++;
256			break;
257		case NGM_VLAN_DEL_FILTER:
258			/* Check that message is long enough. */
259			if (msg->header.arglen != NG_HOOKSIZ) {
260				error = EINVAL;
261				break;
262			}
263			/* Check that hook exists and is active. */
264			hook = ng_findhook(node, (char *)msg->data);
265			if (hook == NULL ||
266			    (f = NG_HOOK_PRIVATE(hook)) == NULL) {
267				error = ENOENT;
268				break;
269			}
270			/* Purge a rule that refers to this hook. */
271			NG_HOOK_SET_PRIVATE(hook, NULL);
272			LIST_REMOVE(f, next);
273			priv->nent--;
274			free(f, M_NETGRAPH);
275			break;
276		case NGM_VLAN_GET_TABLE:
277			NG_MKRESPONSE(resp, msg, sizeof(*t) +
278			    priv->nent * sizeof(*t->filter), M_NOWAIT);
279			if (resp == NULL) {
280				error = ENOMEM;
281				break;
282			}
283			t = (struct ng_vlan_table *)resp->data;
284			t->n = priv->nent;
285			vf = &t->filter[0];
286			for (i = 0; i < HASHSIZE; i++) {
287				LIST_FOREACH(f, &priv->hashtable[i], next) {
288					vf->vlan = f->vlan;
289					strncpy(vf->hook, NG_HOOK_NAME(f->hook),
290					    NG_HOOKSIZ);
291					vf++;
292				}
293			}
294			break;
295		default:		/* Unknown command. */
296			error = EINVAL;
297			break;
298		}
299		break;
300	case NGM_FLOW_COOKIE:
301	    {
302		struct ng_mesg *copy;
303		struct filterhead *chain;
304		struct filter *f;
305
306		/*
307		 * Flow control messages should come only
308		 * from downstream.
309		 */
310
311		if (lasthook == NULL)
312			break;
313		if (lasthook != priv->downstream_hook)
314			break;
315
316		/* Broadcast the event to all uplinks. */
317		for (i = 0, chain = priv->hashtable; i < HASHSIZE;
318		    i++, chain++)
319		LIST_FOREACH(f, chain, next) {
320			NG_COPYMESSAGE(copy, msg, M_NOWAIT);
321			if (copy == NULL)
322				continue;
323			NG_SEND_MSG_HOOK(error, node, copy, f->hook, 0);
324		}
325
326		break;
327	    }
328	default:			/* Unknown type cookie. */
329		error = EINVAL;
330		break;
331	}
332	NG_RESPOND_MSG(error, node, item, resp);
333	NG_FREE_MSG(msg);
334	return (error);
335}
336
337static int
338ng_vlan_rcvdata(hook_p hook, item_p item)
339{
340	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
341	struct ether_header *eh;
342	struct ether_vlan_header *evl = NULL;
343	int error;
344	u_int16_t vlan;
345	struct mbuf *m;
346	struct filter *f;
347
348	/* Make sure we have an entire header. */
349	NGI_GET_M(item, m);
350	if (m->m_len < sizeof(*eh) &&
351	    (m = m_pullup(m, sizeof(*eh))) == NULL) {
352		NG_FREE_ITEM(item);
353		return (EINVAL);
354	}
355	eh = mtod(m, struct ether_header *);
356	if (hook == priv->downstream_hook) {
357		/*
358		 * If from downstream, select between a match hook
359		 * or the nomatch hook.
360		 */
361		if (m->m_flags & M_VLANTAG ||
362		    eh->ether_type == htons(ETHERTYPE_VLAN)) {
363			if (m->m_flags & M_VLANTAG) {
364				/*
365				 * Packet is tagged, m contains a normal
366				 * Ethernet frame; tag is stored out-of-band.
367				 */
368				vlan = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
369			} else {
370				if (m->m_len < sizeof(*evl) &&
371				    (m = m_pullup(m, sizeof(*evl))) == NULL) {
372					NG_FREE_ITEM(item);
373					return (EINVAL);
374				}
375				evl = mtod(m, struct ether_vlan_header *);
376				vlan = EVL_VLANOFTAG(ntohs(evl->evl_tag));
377			}
378			if ((f = ng_vlan_findentry(priv, vlan)) != NULL) {
379				if (m->m_flags & M_VLANTAG) {
380					m->m_pkthdr.ether_vtag = 0;
381					m->m_flags &= ~M_VLANTAG;
382				} else {
383					evl->evl_encap_proto = evl->evl_proto;
384					bcopy(mtod(m, caddr_t),
385					    mtod(m, caddr_t) +
386					    ETHER_VLAN_ENCAP_LEN,
387					    ETHER_HDR_LEN);
388					m_adj(m, ETHER_VLAN_ENCAP_LEN);
389				}
390			}
391		} else
392			f = NULL;
393		if (f != NULL)
394			NG_FWD_NEW_DATA(error, item, f->hook, m);
395		else
396			NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m);
397	} else {
398		/*
399		 * It is heading towards the downstream.
400		 * If from nomatch, pass it unmodified.
401		 * Otherwise, do the VLAN encapsulation.
402		 */
403		if (hook != priv->nomatch_hook) {
404			if ((f = NG_HOOK_PRIVATE(hook)) == NULL) {
405				NG_FREE_ITEM(item);
406				NG_FREE_M(m);
407				return (EOPNOTSUPP);
408			}
409			M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT);
410			/* M_PREPEND takes care of m_len and m_pkthdr.len. */
411			if (m == NULL || (m->m_len < sizeof(*evl) &&
412			    (m = m_pullup(m, sizeof(*evl))) == NULL)) {
413				NG_FREE_ITEM(item);
414				return (ENOMEM);
415			}
416			/*
417			 * Transform the Ethernet header into an Ethernet header
418			 * with 802.1Q encapsulation.
419			 */
420			bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN,
421			    mtod(m, char *), ETHER_HDR_LEN);
422			evl = mtod(m, struct ether_vlan_header *);
423			evl->evl_proto = evl->evl_encap_proto;
424			evl->evl_encap_proto = htons(ETHERTYPE_VLAN);
425			evl->evl_tag = htons(f->vlan);
426		}
427		NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m);
428	}
429	return (error);
430}
431
432static int
433ng_vlan_shutdown(node_p node)
434{
435	const priv_p priv = NG_NODE_PRIVATE(node);
436
437	NG_NODE_SET_PRIVATE(node, NULL);
438	NG_NODE_UNREF(node);
439	free(priv, M_NETGRAPH);
440	return (0);
441}
442
443static int
444ng_vlan_disconnect(hook_p hook)
445{
446	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
447	struct filter *f;
448
449	if (hook == priv->downstream_hook)
450		priv->downstream_hook = NULL;
451	else if (hook == priv->nomatch_hook)
452		priv->nomatch_hook = NULL;
453	else {
454		/* Purge a rule that refers to this hook. */
455		if ((f = NG_HOOK_PRIVATE(hook)) != NULL) {
456			LIST_REMOVE(f, next);
457			priv->nent--;
458			free(f, M_NETGRAPH);
459		}
460	}
461	NG_HOOK_SET_PRIVATE(hook, NULL);
462	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
463	    (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
464		ng_rmnode_self(NG_HOOK_NODE(hook));
465	return (0);
466}
467