mirror of
https://github.com/freebsd/freebsd-src
synced 2024-09-06 17:18:32 +00:00
509 lines
12 KiB
C
509 lines
12 KiB
C
/*-
|
|
* SPDX-License-Identifier: BSD-2-Clause
|
|
*
|
|
* Copyright (c) 2019-2021 IKS Service GmbH
|
|
*
|
|
* 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: Lutz Donnerhacke <lutz@donnerhacke.de>
|
|
*/
|
|
|
|
#include <sys/param.h>
|
|
#include <sys/systm.h>
|
|
#include <sys/kernel.h>
|
|
#include <sys/mbuf.h>
|
|
#include <sys/malloc.h>
|
|
#include <sys/ctype.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/syslog.h>
|
|
#include <sys/types.h>
|
|
#include <sys/counter.h>
|
|
|
|
#include <net/ethernet.h>
|
|
|
|
#include <netgraph/ng_message.h>
|
|
#include <netgraph/ng_parse.h>
|
|
#include <netgraph/ng_vlan_rotate.h>
|
|
#include <netgraph/netgraph.h>
|
|
|
|
/*
|
|
* This section contains the netgraph method declarations for the
|
|
* sample node. These methods define the netgraph 'type'.
|
|
*/
|
|
|
|
static ng_constructor_t ng_vlanrotate_constructor;
|
|
static ng_rcvmsg_t ng_vlanrotate_rcvmsg;
|
|
static ng_shutdown_t ng_vlanrotate_shutdown;
|
|
static ng_newhook_t ng_vlanrotate_newhook;
|
|
static ng_rcvdata_t ng_vlanrotate_rcvdata;
|
|
static ng_disconnect_t ng_vlanrotate_disconnect;
|
|
|
|
/* Parse type for struct ng_vlanrotate_conf */
|
|
static const struct ng_parse_struct_field ng_vlanrotate_conf_fields[] = {
|
|
{"rot", &ng_parse_int8_type},
|
|
{"min", &ng_parse_uint8_type},
|
|
{"max", &ng_parse_uint8_type},
|
|
{NULL}
|
|
};
|
|
static const struct ng_parse_type ng_vlanrotate_conf_type = {
|
|
&ng_parse_struct_type,
|
|
&ng_vlanrotate_conf_fields
|
|
};
|
|
|
|
/* Parse type for struct ng_vlanrotate_stat */
|
|
static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = {
|
|
&ng_parse_uint64_type,
|
|
NG_VLANROTATE_MAX_VLANS
|
|
};
|
|
static struct ng_parse_type ng_vlanrotate_stat_hist = {
|
|
&ng_parse_fixedarray_type,
|
|
&ng_vlanrotate_stat_hist_info
|
|
};
|
|
static const struct ng_parse_struct_field ng_vlanrotate_stat_fields[] = {
|
|
{"drops", &ng_parse_uint64_type},
|
|
{"excessive", &ng_parse_uint64_type},
|
|
{"incomplete", &ng_parse_uint64_type},
|
|
{"histogram", &ng_vlanrotate_stat_hist},
|
|
{NULL}
|
|
};
|
|
static struct ng_parse_type ng_vlanrotate_stat_type = {
|
|
&ng_parse_struct_type,
|
|
&ng_vlanrotate_stat_fields
|
|
};
|
|
|
|
|
|
/* List of commands and how to convert arguments to/from ASCII */
|
|
static const struct ng_cmdlist ng_vlanrotate_cmdlist[] = {
|
|
{
|
|
NGM_VLANROTATE_COOKIE,
|
|
NGM_VLANROTATE_GET_CONF,
|
|
"getconf",
|
|
NULL,
|
|
&ng_vlanrotate_conf_type,
|
|
},
|
|
{
|
|
NGM_VLANROTATE_COOKIE,
|
|
NGM_VLANROTATE_SET_CONF,
|
|
"setconf",
|
|
&ng_vlanrotate_conf_type,
|
|
NULL
|
|
},
|
|
{
|
|
NGM_VLANROTATE_COOKIE,
|
|
NGM_VLANROTATE_GET_STAT,
|
|
"getstat",
|
|
NULL,
|
|
&ng_vlanrotate_stat_type
|
|
},
|
|
{
|
|
NGM_VLANROTATE_COOKIE,
|
|
NGM_VLANROTATE_CLR_STAT,
|
|
"clrstat",
|
|
NULL,
|
|
&ng_vlanrotate_stat_type
|
|
},
|
|
{
|
|
NGM_VLANROTATE_COOKIE,
|
|
NGM_VLANROTATE_GETCLR_STAT,
|
|
"getclrstat",
|
|
NULL,
|
|
&ng_vlanrotate_stat_type
|
|
},
|
|
{0}
|
|
};
|
|
|
|
/* Netgraph node type descriptor */
|
|
static struct ng_type typestruct = {
|
|
.version = NG_ABI_VERSION,
|
|
.name = NG_VLANROTATE_NODE_TYPE,
|
|
.constructor = ng_vlanrotate_constructor,
|
|
.rcvmsg = ng_vlanrotate_rcvmsg,
|
|
.shutdown = ng_vlanrotate_shutdown,
|
|
.newhook = ng_vlanrotate_newhook,
|
|
.rcvdata = ng_vlanrotate_rcvdata,
|
|
.disconnect = ng_vlanrotate_disconnect,
|
|
.cmdlist = ng_vlanrotate_cmdlist,
|
|
};
|
|
NETGRAPH_INIT(vlanrotate, &typestruct);
|
|
|
|
struct ng_vlanrotate_kernel_stats {
|
|
counter_u64_t drops, excessive, incomplete;
|
|
counter_u64_t histogram[NG_VLANROTATE_MAX_VLANS];
|
|
};
|
|
|
|
/* Information we store for each node */
|
|
struct vlanrotate {
|
|
hook_p original_hook;
|
|
hook_p ordered_hook;
|
|
hook_p excessive_hook;
|
|
hook_p incomplete_hook;
|
|
struct ng_vlanrotate_conf conf;
|
|
struct ng_vlanrotate_kernel_stats stats;
|
|
};
|
|
typedef struct vlanrotate *vlanrotate_p;
|
|
|
|
/*
|
|
* Set up the private data structure.
|
|
*/
|
|
static int
|
|
ng_vlanrotate_constructor(node_p node)
|
|
{
|
|
int i;
|
|
|
|
vlanrotate_p vrp = malloc(sizeof(*vrp), M_NETGRAPH, M_WAITOK | M_ZERO);
|
|
|
|
vrp->conf.max = NG_VLANROTATE_MAX_VLANS;
|
|
|
|
vrp->stats.drops = counter_u64_alloc(M_WAITOK);
|
|
vrp->stats.excessive = counter_u64_alloc(M_WAITOK);
|
|
vrp->stats.incomplete = counter_u64_alloc(M_WAITOK);
|
|
for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
|
|
vrp->stats.histogram[i] = counter_u64_alloc(M_WAITOK);
|
|
|
|
NG_NODE_SET_PRIVATE(node, vrp);
|
|
return (0);
|
|
}
|
|
|
|
/*
|
|
* Give our ok for a hook to be added.
|
|
*/
|
|
static int
|
|
ng_vlanrotate_newhook(node_p node, hook_p hook, const char *name)
|
|
{
|
|
const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
|
|
hook_p *dst = NULL;
|
|
|
|
if (strcmp(name, NG_VLANROTATE_HOOK_ORDERED) == 0) {
|
|
dst = &vrp->ordered_hook;
|
|
} else if (strcmp(name, NG_VLANROTATE_HOOK_ORIGINAL) == 0) {
|
|
dst = &vrp->original_hook;
|
|
} else if (strcmp(name, NG_VLANROTATE_HOOK_EXCESSIVE) == 0) {
|
|
dst = &vrp->excessive_hook;
|
|
} else if (strcmp(name, NG_VLANROTATE_HOOK_INCOMPLETE) == 0) {
|
|
dst = &vrp->incomplete_hook;
|
|
} else
|
|
return (EINVAL); /* not a hook we know about */
|
|
|
|
if (*dst != NULL)
|
|
return (EADDRINUSE); /* don't override */
|
|
|
|
*dst = hook;
|
|
return (0);
|
|
}
|
|
|
|
/*
|
|
* Get a netgraph control message.
|
|
* A response is not required.
|
|
*/
|
|
static int
|
|
ng_vlanrotate_rcvmsg(node_p node, item_p item, hook_p lasthook)
|
|
{
|
|
const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
|
|
struct ng_mesg *resp = NULL;
|
|
struct ng_mesg *msg;
|
|
struct ng_vlanrotate_conf *pcf;
|
|
int error = 0;
|
|
|
|
NGI_GET_MSG(item, msg);
|
|
/* Deal with message according to cookie and command */
|
|
switch (msg->header.typecookie) {
|
|
case NGM_VLANROTATE_COOKIE:
|
|
switch (msg->header.cmd) {
|
|
case NGM_VLANROTATE_GET_CONF:
|
|
NG_MKRESPONSE(resp, msg, sizeof(vrp->conf), M_NOWAIT);
|
|
if (!resp) {
|
|
error = ENOMEM;
|
|
break;
|
|
}
|
|
*((struct ng_vlanrotate_conf *)resp->data) = vrp->conf;
|
|
break;
|
|
case NGM_VLANROTATE_SET_CONF:
|
|
if (msg->header.arglen != sizeof(*pcf)) {
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
|
|
pcf = (struct ng_vlanrotate_conf *)msg->data;
|
|
|
|
if (pcf->max == 0) /* keep current value */
|
|
pcf->max = vrp->conf.max;
|
|
|
|
if ((pcf->max > NG_VLANROTATE_MAX_VLANS) ||
|
|
(pcf->min > pcf->max) ||
|
|
(abs(pcf->rot) >= pcf->max)) {
|
|
error = EINVAL;
|
|
break;
|
|
}
|
|
|
|
vrp->conf = *pcf;
|
|
break;
|
|
case NGM_VLANROTATE_GET_STAT:
|
|
case NGM_VLANROTATE_GETCLR_STAT:
|
|
{
|
|
struct ng_vlanrotate_stat *p;
|
|
int i;
|
|
|
|
NG_MKRESPONSE(resp, msg, sizeof(*p), M_NOWAIT);
|
|
if (!resp) {
|
|
error = ENOMEM;
|
|
break;
|
|
}
|
|
p = (struct ng_vlanrotate_stat *)resp->data;
|
|
p->drops = counter_u64_fetch(vrp->stats.drops);
|
|
p->excessive = counter_u64_fetch(vrp->stats.excessive);
|
|
p->incomplete = counter_u64_fetch(vrp->stats.incomplete);
|
|
for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
|
|
p->histogram[i] = counter_u64_fetch(vrp->stats.histogram[i]);
|
|
if (msg->header.cmd != NGM_VLANROTATE_GETCLR_STAT)
|
|
break;
|
|
}
|
|
case NGM_VLANROTATE_CLR_STAT:
|
|
{
|
|
int i;
|
|
|
|
counter_u64_zero(vrp->stats.drops);
|
|
counter_u64_zero(vrp->stats.excessive);
|
|
counter_u64_zero(vrp->stats.incomplete);
|
|
for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
|
|
counter_u64_zero(vrp->stats.histogram[i]);
|
|
break;
|
|
}
|
|
default:
|
|
error = EINVAL; /* unknown command */
|
|
break;
|
|
}
|
|
break;
|
|
default:
|
|
error = EINVAL; /* unknown cookie type */
|
|
break;
|
|
}
|
|
|
|
/* Take care of synchronous response, if any */
|
|
NG_RESPOND_MSG(error, node, item, resp);
|
|
/* Free the message and return */
|
|
NG_FREE_MSG(msg);
|
|
return (error);
|
|
}
|
|
|
|
/*
|
|
* Receive data, and do rotate the vlans as desired.
|
|
*
|
|
* Rotating is quite complicated if the rotation offset and the number
|
|
* of vlans are not relativly prime. In this case multiple slices need
|
|
* to be rotated separately.
|
|
*
|
|
* Rotation can be additive or subtractive. Some examples:
|
|
* 01234 5 vlans given
|
|
* -----
|
|
* 34012 +2 rotate
|
|
* 12340 +4 rotate
|
|
* 12340 -1 rotate
|
|
*
|
|
* First some helper functions ...
|
|
*/
|
|
|
|
struct ether_vlan_stack_entry {
|
|
uint16_t proto;
|
|
uint16_t tag;
|
|
} __packed;
|
|
|
|
struct ether_vlan_stack_header {
|
|
uint8_t dst[ETHER_ADDR_LEN];
|
|
uint8_t src[ETHER_ADDR_LEN];
|
|
struct ether_vlan_stack_entry vlan_stack[1];
|
|
} __packed;
|
|
|
|
static int
|
|
ng_vlanrotate_gcd(int a, int b)
|
|
{
|
|
if (b == 0)
|
|
return a;
|
|
else
|
|
return ng_vlanrotate_gcd(b, a % b);
|
|
}
|
|
|
|
static void
|
|
ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[], int d, int n)
|
|
{
|
|
int i, j, k;
|
|
struct ether_vlan_stack_entry temp;
|
|
|
|
/* for each commensurable slice */
|
|
for (i = ng_vlanrotate_gcd(d, n); i-- > 0;) {
|
|
/* rotate left aka downwards */
|
|
temp = arr[i];
|
|
j = i;
|
|
|
|
while (1) {
|
|
k = j + d;
|
|
if (k >= n)
|
|
k = k - n;
|
|
if (k == i)
|
|
break;
|
|
arr[j] = arr[k];
|
|
j = k;
|
|
}
|
|
|
|
arr[j] = temp;
|
|
}
|
|
}
|
|
|
|
static int
|
|
ng_vlanrotate_rcvdata(hook_p hook, item_p item)
|
|
{
|
|
const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
|
|
struct ether_vlan_stack_header *evsh;
|
|
struct mbuf *m = NULL;
|
|
hook_p dst_hook;
|
|
int8_t rotate;
|
|
int8_t vlans = 0;
|
|
int error = ENOSYS;
|
|
|
|
NGI_GET_M(item, m);
|
|
|
|
if (hook == vrp->ordered_hook) {
|
|
rotate = +vrp->conf.rot;
|
|
dst_hook = vrp->original_hook;
|
|
} else if (hook == vrp->original_hook) {
|
|
rotate = -vrp->conf.rot;
|
|
dst_hook = vrp->ordered_hook;
|
|
} else {
|
|
dst_hook = vrp->original_hook;
|
|
goto send; /* everything else goes out unmodified */
|
|
}
|
|
|
|
if (dst_hook == NULL) {
|
|
error = ENETDOWN;
|
|
goto fail;
|
|
}
|
|
|
|
/* count the vlans */
|
|
for (vlans = 0; vlans <= NG_VLANROTATE_MAX_VLANS; vlans++) {
|
|
size_t expected_len = sizeof(struct ether_vlan_stack_header)
|
|
+ vlans * sizeof(struct ether_vlan_stack_entry);
|
|
|
|
if (m->m_len < expected_len) {
|
|
m = m_pullup(m, expected_len);
|
|
if (m == NULL) {
|
|
error = EINVAL;
|
|
goto fail;
|
|
}
|
|
}
|
|
|
|
evsh = mtod(m, struct ether_vlan_stack_header *);
|
|
switch (ntohs(evsh->vlan_stack[vlans].proto)) {
|
|
case ETHERTYPE_VLAN:
|
|
case ETHERTYPE_QINQ:
|
|
case ETHERTYPE_8021Q9100:
|
|
case ETHERTYPE_8021Q9200:
|
|
case ETHERTYPE_8021Q9300:
|
|
break;
|
|
default:
|
|
goto out;
|
|
}
|
|
}
|
|
out:
|
|
if ((vlans > vrp->conf.max) || (vlans >= NG_VLANROTATE_MAX_VLANS)) {
|
|
counter_u64_add(vrp->stats.excessive, 1);
|
|
dst_hook = vrp->excessive_hook;
|
|
goto send;
|
|
}
|
|
|
|
if ((vlans < vrp->conf.min) || (vlans <= abs(rotate))) {
|
|
counter_u64_add(vrp->stats.incomplete, 1);
|
|
dst_hook = vrp->incomplete_hook;
|
|
goto send;
|
|
}
|
|
counter_u64_add(vrp->stats.histogram[vlans], 1);
|
|
|
|
/* rotating upwards always (using modular arithmetics) */
|
|
if (rotate == 0) {
|
|
/* nothing to do */
|
|
} else if (rotate > 0) {
|
|
ng_vlanrotate_rotate(evsh->vlan_stack, rotate, vlans);
|
|
} else {
|
|
ng_vlanrotate_rotate(evsh->vlan_stack, vlans + rotate, vlans);
|
|
}
|
|
|
|
send:
|
|
if (dst_hook == NULL)
|
|
goto fail;
|
|
NG_FWD_NEW_DATA(error, item, dst_hook, m);
|
|
return 0;
|
|
|
|
fail:
|
|
counter_u64_add(vrp->stats.drops, 1);
|
|
if (m != NULL)
|
|
m_freem(m);
|
|
NG_FREE_ITEM(item);
|
|
return (error);
|
|
}
|
|
|
|
/*
|
|
* Do local shutdown processing..
|
|
* All our links and the name have already been removed.
|
|
*/
|
|
static int
|
|
ng_vlanrotate_shutdown(node_p node)
|
|
{
|
|
const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
|
|
int i;
|
|
|
|
NG_NODE_SET_PRIVATE(node, NULL);
|
|
|
|
counter_u64_free(vrp->stats.drops);
|
|
counter_u64_free(vrp->stats.excessive);
|
|
counter_u64_free(vrp->stats.incomplete);
|
|
for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
|
|
counter_u64_free(vrp->stats.histogram[i]);
|
|
|
|
free(vrp, M_NETGRAPH);
|
|
|
|
NG_NODE_UNREF(node);
|
|
return (0);
|
|
}
|
|
|
|
/*
|
|
* Hook disconnection
|
|
* For this type, removal of the last link destroys the node
|
|
*/
|
|
static int
|
|
ng_vlanrotate_disconnect(hook_p hook)
|
|
{
|
|
const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
|
|
|
|
if (vrp->original_hook == hook)
|
|
vrp->original_hook = NULL;
|
|
if (vrp->ordered_hook == hook)
|
|
vrp->ordered_hook = NULL;
|
|
if (vrp->excessive_hook == hook)
|
|
vrp->excessive_hook = NULL;
|
|
if (vrp->incomplete_hook == hook)
|
|
vrp->incomplete_hook = NULL;
|
|
|
|
/* during shutdown the node is invalid, don't shutdown twice */
|
|
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);
|
|
}
|