053359b7f4
No functional change.
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 response. */
|
|
NG_MKRESPONSE(resp, msg, sizeof(*t) +
|
|
vlan_count * sizeof(*t->filter), M_NOWAIT);
|
|
if (resp == NULL) {
|
|
error = ENOMEM;
|
|
break;
|
|
}
|
|
|
|
/* Pack data to response. */
|
|
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: always 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_NOWAIT);
|
|
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);
|
|
}
|