f3886e29af
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>
711 lines
18 KiB
C
711 lines
18 KiB
C
/*-
|
|
* 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
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
|
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
|
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
* SUCH DAMAGE.
|
|
*
|
|
* Author: Ruslan Ermilov <ru@FreeBSD.org>
|
|
*
|
|
* $FreeBSD$
|
|
*/
|
|
|
|
#include <sys/param.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/kernel.h>
|
|
#include <sys/malloc.h>
|
|
#include <sys/mbuf.h>
|
|
#include <sys/queue.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/systm.h>
|
|
|
|
#include <net/ethernet.h>
|
|
#include <net/if.h>
|
|
#include <net/if_vlan_var.h>
|
|
|
|
#include <netgraph/ng_message.h>
|
|
#include <netgraph/ng_parse.h>
|
|
#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;
|
|
static ng_newhook_t ng_vlan_newhook;
|
|
static ng_rcvdata_t ng_vlan_rcvdata;
|
|
static ng_disconnect_t ng_vlan_disconnect;
|
|
|
|
/* Parse type for struct ng_vlan_filter. */
|
|
static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
|
|
NG_VLAN_FILTER_FIELDS;
|
|
static const struct ng_parse_type ng_vlan_filter_type = {
|
|
&ng_parse_struct_type,
|
|
&ng_vlan_filter_fields
|
|
};
|
|
|
|
static int
|
|
ng_vlan_getTableLength(const struct ng_parse_type *type,
|
|
const u_char *start, const u_char *buf)
|
|
{
|
|
const struct ng_vlan_table *const table =
|
|
(const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
|
|
|
|
return table->n;
|
|
}
|
|
|
|
/* Parse type for struct ng_vlan_table. */
|
|
static const struct ng_parse_array_info ng_vlan_table_array_info = {
|
|
&ng_vlan_filter_type,
|
|
ng_vlan_getTableLength
|
|
};
|
|
static const struct ng_parse_type ng_vlan_table_array_type = {
|
|
&ng_parse_array_type,
|
|
&ng_vlan_table_array_info
|
|
};
|
|
static const struct ng_parse_struct_field ng_vlan_table_fields[] =
|
|
NG_VLAN_TABLE_FIELDS;
|
|
static const struct ng_parse_type ng_vlan_table_type = {
|
|
&ng_parse_struct_type,
|
|
&ng_vlan_table_fields
|
|
};
|
|
|
|
/* List of commands and how to convert arguments to/from ASCII. */
|
|
static const struct ng_cmdlist ng_vlan_cmdlist[] = {
|
|
{
|
|
NGM_VLAN_COOKIE,
|
|
NGM_VLAN_ADD_FILTER,
|
|
"addfilter",
|
|
&ng_vlan_filter_type,
|
|
NULL
|
|
},
|
|
{
|
|
NGM_VLAN_COOKIE,
|
|
NGM_VLAN_DEL_FILTER,
|
|
"delfilter",
|
|
&ng_parse_hookbuf_type,
|
|
NULL
|
|
},
|
|
{
|
|
NGM_VLAN_COOKIE,
|
|
NGM_VLAN_GET_TABLE,
|
|
"gettable",
|
|
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 }
|
|
};
|
|
|
|
static struct ng_type ng_vlan_typestruct = {
|
|
.version = NG_ABI_VERSION,
|
|
.name = NG_VLAN_NODE_TYPE,
|
|
.constructor = ng_vlan_constructor,
|
|
.rcvmsg = ng_vlan_rcvmsg,
|
|
.shutdown = ng_vlan_shutdown,
|
|
.newhook = ng_vlan_newhook,
|
|
.rcvdata = ng_vlan_rcvdata,
|
|
.disconnect = ng_vlan_disconnect,
|
|
.cmdlist = ng_vlan_cmdlist,
|
|
};
|
|
NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
|
|
|
|
|
|
/*
|
|
* Helper functions.
|
|
*/
|
|
|
|
static __inline int
|
|
m_chk(struct mbuf **mp, int len)
|
|
{
|
|
|
|
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;
|
|
|
|
priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
|
|
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);
|
|
}
|
|
|
|
static int
|
|
ng_vlan_newhook(node_p node, hook_p hook, const char *name)
|
|
{
|
|
const priv_p priv = NG_NODE_PRIVATE(node);
|
|
|
|
if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
|
|
priv->downstream_hook = hook;
|
|
else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
|
|
priv->nomatch_hook = hook;
|
|
else {
|
|
/*
|
|
* Any other hook name is valid and can
|
|
* later be associated with a filter rule.
|
|
*/
|
|
}
|
|
NG_HOOK_SET_PRIVATE(hook, NULL);
|
|
return (0);
|
|
}
|
|
|
|
static int
|
|
ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
|
|
{
|
|
const priv_p priv = NG_NODE_PRIVATE(node);
|
|
struct ng_mesg *msg, *resp = NULL;
|
|
struct ng_vlan_filter *vf;
|
|
hook_p hook;
|
|
struct ng_vlan_table *t;
|
|
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. */
|
|
switch (msg->header.typecookie) {
|
|
case NGM_VLAN_COOKIE:
|
|
switch (msg->header.cmd) {
|
|
case NGM_VLAN_ADD_FILTER:
|
|
/* Check that message is long enough. */
|
|
if (msg->header.arglen != sizeof(*vf)) {
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
vf = (struct ng_vlan_filter *)msg->data;
|
|
/* Sanity check the VLAN ID value. */
|
|
#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_name);
|
|
if (hook == NULL) {
|
|
error = ENOENT;
|
|
break;
|
|
}
|
|
/* And is not one of the special hooks. */
|
|
if (hook == priv->downstream_hook ||
|
|
hook == priv->nomatch_hook) {
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
/* And is not already in service. */
|
|
if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
|
|
error = EEXIST;
|
|
break;
|
|
}
|
|
/* Check we don't already trap this VLAN. */
|
|
if (priv->vlan_hook[vf->vid] != NULL) {
|
|
error = EEXIST;
|
|
break;
|
|
}
|
|
/* 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. */
|
|
if (msg->header.arglen != NG_HOOKSIZ) {
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
/* Check that hook exists and is active. */
|
|
hook = ng_findhook(node, (char *)msg->data);
|
|
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);
|
|
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) +
|
|
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 = 0;
|
|
vf = &t->filter[0];
|
|
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;
|
|
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;
|
|
}
|
|
break;
|
|
case NGM_FLOW_COOKIE:
|
|
{
|
|
struct ng_mesg *copy;
|
|
|
|
/*
|
|
* Flow control messages should come only
|
|
* from downstream.
|
|
*/
|
|
|
|
if (lasthook == NULL)
|
|
break;
|
|
if (lasthook != priv->downstream_hook)
|
|
break;
|
|
/* Broadcast the event to all uplinks. */
|
|
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,
|
|
priv->vlan_hook[i], 0);
|
|
}
|
|
break;
|
|
}
|
|
default: /* Unknown type cookie. */
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
NG_RESPOND_MSG(error, node, item, resp);
|
|
NG_FREE_MSG(msg);
|
|
return (error);
|
|
}
|
|
|
|
static int
|
|
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;
|
|
int error;
|
|
uintptr_t hook_data;
|
|
uint16_t vid, eth_vtag;
|
|
struct mbuf *m;
|
|
hook_p dst_hook;
|
|
|
|
|
|
NGI_GET_M(item, m);
|
|
|
|
/* 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.
|
|
*/
|
|
|
|
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.
|
|
*/
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
|
|
/*
|
|
* 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);
|
|
}
|
|
|
|
static int
|
|
ng_vlan_shutdown(node_p node)
|
|
{
|
|
const priv_p priv = NG_NODE_PRIVATE(node);
|
|
|
|
NG_NODE_SET_PRIVATE(node, NULL);
|
|
NG_NODE_UNREF(node);
|
|
free(priv, M_NETGRAPH);
|
|
return (0);
|
|
}
|
|
|
|
static int
|
|
ng_vlan_disconnect(hook_p hook)
|
|
{
|
|
const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
|
|
uintptr_t hook_data;
|
|
|
|
if (hook == priv->downstream_hook)
|
|
priv->downstream_hook = NULL;
|
|
else if (hook == priv->nomatch_hook)
|
|
priv->nomatch_hook = NULL;
|
|
else {
|
|
/* Purge a rule that refers to this hook. */
|
|
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) &&
|
|
(NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
|
|
ng_rmnode_self(NG_HOOK_NODE(hook));
|
|
return (0);
|
|
}
|