freebsd-nq/sys/netgraph/ng_patch.c

/*-
 * Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com>
 * Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru>
 * 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.
 *
 */

#include <sys/cdefs.h>
__FBSDID("$FreeBSD$");

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/endian.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>

#include <net/bpf.h>
#include <net/ethernet.h>

#include <netgraph/ng_message.h>
#include <netgraph/ng_parse.h>
#include <netgraph/netgraph.h>

#include <netgraph/ng_patch.h>

/* private data */
struct ng_patch_priv {
	hook_p		in;
	hook_p		out;
	uint8_t		dlt;	/* DLT_XXX from bpf.h */
	struct ng_patch_stats stats;
	struct ng_patch_config *conf;
};

typedef struct ng_patch_priv *priv_p;

/* Netgraph methods */
static ng_constructor_t	ng_patch_constructor;
static ng_rcvmsg_t	ng_patch_rcvmsg;
static ng_shutdown_t	ng_patch_shutdown;
static ng_newhook_t	ng_patch_newhook;
static ng_rcvdata_t	ng_patch_rcvdata;
static ng_disconnect_t	ng_patch_disconnect;

#define ERROUT(x) { error = (x); goto done; }

static int
ng_patch_config_getlen(const struct ng_parse_type *type,
    const u_char *start, const u_char *buf)
{
	const struct ng_patch_config *conf;

	conf = (const struct ng_patch_config *)(buf -
	    offsetof(struct ng_patch_config, ops));

	return (conf->count);
}

static const struct ng_parse_struct_field ng_patch_op_type_fields[]
	= NG_PATCH_OP_TYPE;
static const struct ng_parse_type ng_patch_op_type = {
	&ng_parse_struct_type,
	&ng_patch_op_type_fields
};

static const struct ng_parse_array_info ng_patch_ops_array_info = {
	&ng_patch_op_type,
	&ng_patch_config_getlen
};
static const struct ng_parse_type ng_patch_ops_array_type = {
	&ng_parse_array_type,
	&ng_patch_ops_array_info
};

static const struct ng_parse_struct_field ng_patch_config_type_fields[]
	= NG_PATCH_CONFIG_TYPE;
static const struct ng_parse_type ng_patch_config_type = {
	&ng_parse_struct_type,
	&ng_patch_config_type_fields
};

static const struct ng_parse_struct_field ng_patch_stats_fields[]
	= NG_PATCH_STATS_TYPE;
static const struct ng_parse_type ng_patch_stats_type = {
	&ng_parse_struct_type,
	&ng_patch_stats_fields
};

static const struct ng_cmdlist ng_patch_cmdlist[] = {
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_GETDLT,
		"getdlt",
		NULL,
		&ng_parse_uint8_type
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_SETDLT,
		"setdlt",
		&ng_parse_uint8_type,
		NULL
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_GETCONFIG,
		"getconfig",
		NULL,
		&ng_patch_config_type
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_SETCONFIG,
		"setconfig",
		&ng_patch_config_type,
		NULL
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_GET_STATS,
		"getstats",
		NULL,
		&ng_patch_stats_type
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_CLR_STATS,
		"clrstats",
		NULL,
		NULL
	},
	{
		NGM_PATCH_COOKIE,
		NGM_PATCH_GETCLR_STATS,
		"getclrstats",
		NULL,
		&ng_patch_stats_type
	},
	{ 0 }
};

static struct ng_type typestruct = {
	.version =	NG_ABI_VERSION,
	.name =		NG_PATCH_NODE_TYPE,
	.constructor =	ng_patch_constructor,
	.rcvmsg =	ng_patch_rcvmsg,
	.shutdown =	ng_patch_shutdown,
	.newhook =	ng_patch_newhook,
	.rcvdata =	ng_patch_rcvdata,
	.disconnect =	ng_patch_disconnect,
	.cmdlist =	ng_patch_cmdlist,
};

NETGRAPH_INIT(patch, &typestruct);

static int
ng_patch_constructor(node_p node)
{
	priv_p privdata;

	privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO);
	privdata->dlt = DLT_RAW;

	NG_NODE_SET_PRIVATE(node, privdata);

	return (0);
}

static int
ng_patch_newhook(node_p node, hook_p hook, const char *name)
{
	const priv_p privp = NG_NODE_PRIVATE(node);

	if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) {
		privp->in = hook;
	} else if (strncmp(name, NG_PATCH_HOOK_OUT,
	    strlen(NG_PATCH_HOOK_OUT)) == 0) {
		privp->out = hook;
	} else
		return (EINVAL);

	return (0);
}

static int
ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
	const priv_p privp = NG_NODE_PRIVATE(node);
	struct ng_patch_config *conf, *newconf;
	struct ng_mesg *msg;
	struct ng_mesg *resp = NULL;
	int i, error = 0;

	NGI_GET_MSG(item, msg);

	if  (msg->header.typecookie != NGM_PATCH_COOKIE)
		ERROUT(EINVAL);

	switch (msg->header.cmd)
	{
		case NGM_PATCH_GETCONFIG:
			if (privp->conf == NULL)
				ERROUT(0);

			NG_MKRESPONSE(resp, msg,
			    NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK);

			if (resp == NULL)
				ERROUT(ENOMEM);

			bcopy(privp->conf, resp->data,
			    NG_PATCH_CONF_SIZE(privp->conf->count));

			conf = (struct ng_patch_config *) resp->data;

			for (i = 0; i < conf->count; i++) {
				switch (conf->ops[i].length)
				{
					case 1:
						conf->ops[i].val.v8 = conf->ops[i].val.v1;
						break;
					case 2:
						conf->ops[i].val.v8 = conf->ops[i].val.v2;
						break;
					case 4:
						conf->ops[i].val.v8 = conf->ops[i].val.v4;
						break;
					case 8:
						break;
				}
			}

			break;

		case NGM_PATCH_SETCONFIG:
			conf = (struct ng_patch_config *) msg->data;

			if (msg->header.arglen < sizeof(struct ng_patch_config) ||
			    msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count))
				ERROUT(EINVAL);

			for (i = 0; i < conf->count; i++) {
				switch (conf->ops[i].length)
				{
					case 1:
						conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8;
						break;
					case 2:
						conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8;
						break;
					case 4:
						conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8;
						break;
					case 8:
						break;
					default:
						ERROUT(EINVAL);
				}
			}

			conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6;
			conf->relative_offset = !!conf->relative_offset;

			newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO);

			bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count));

			if (privp->conf)
				free(privp->conf, M_NETGRAPH);

			privp->conf = newconf;

			break;

		case NGM_PATCH_GET_STATS:
		case NGM_PATCH_CLR_STATS:
		case NGM_PATCH_GETCLR_STATS:
			if (msg->header.cmd != NGM_PATCH_CLR_STATS) {
				NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK);

				if (resp == NULL)
					ERROUT(ENOMEM);

				bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats));
			}

			if (msg->header.cmd != NGM_PATCH_GET_STATS)
				bzero(&(privp->stats), sizeof(struct ng_patch_stats));

			break;

		case NGM_PATCH_GETDLT:
			NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK);

			if (resp == NULL)
				ERROUT(ENOMEM);

			*((uint8_t *) resp->data) = privp->dlt;

			break;

		case NGM_PATCH_SETDLT:
			if (msg->header.arglen != sizeof(uint8_t))
				ERROUT(EINVAL);

			switch (*(uint8_t *) msg->data)
			{
				case DLT_EN10MB:
				case DLT_RAW:
					privp->dlt = *(uint8_t *) msg->data;
					break;

				default:
					ERROUT(EINVAL);
			}

			break;

		default:
			ERROUT(EINVAL);
	}

done:
	NG_RESPOND_MSG(error, node, item, resp);
	NG_FREE_MSG(msg);

	return (error);
}

static void
do_patch(priv_p privp, struct mbuf *m, int global_offset)
{
	int i, offset, patched = 0;
	union ng_patch_op_val val;

	for (i = 0; i < privp->conf->count; i++) {
		offset = global_offset + privp->conf->ops[i].offset;

		if (offset + privp->conf->ops[i].length > m->m_pkthdr.len)
			continue;

		/* for "=" operation we don't need to copy data from mbuf */
		if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET)
			m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val);

		switch (privp->conf->ops[i].length)
		{
			case 1:
				switch (privp->conf->ops[i].mode)
				{
					case NG_PATCH_MODE_SET:
						val.v1 = privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_ADD:
						val.v1 += privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_SUB:
						val.v1 -= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_MUL:
						val.v1 *= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_DIV:
						val.v1 /= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_NEG:
						*((int8_t *) &val) = - *((int8_t *) &val);
						break;
					case NG_PATCH_MODE_AND:
						val.v1 &= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_OR:
						val.v1 |= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_XOR:
						val.v1 ^= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_SHL:
						val.v1 <<= privp->conf->ops[i].val.v1;
						break;
					case NG_PATCH_MODE_SHR:
						val.v1 >>= privp->conf->ops[i].val.v1;
						break;
				}
				break;

			case 2:
				val.v2 = ntohs(val.v2);

				switch (privp->conf->ops[i].mode)
				{
					case NG_PATCH_MODE_SET:
						val.v2 = privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_ADD:
						val.v2 += privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_SUB:
						val.v2 -= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_MUL:
						val.v2 *= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_DIV:
						val.v2 /= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_NEG:
						*((int16_t *) &val) = - *((int16_t *) &val);
						break;
					case NG_PATCH_MODE_AND:
						val.v2 &= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_OR:
						val.v2 |= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_XOR:
						val.v2 ^= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_SHL:
						val.v2 <<= privp->conf->ops[i].val.v2;
						break;
					case NG_PATCH_MODE_SHR:
						val.v2 >>= privp->conf->ops[i].val.v2;
						break;
				}

				val.v2 = htons(val.v2);

				break;

			case 4:
				val.v4 = ntohl(val.v4);

				switch (privp->conf->ops[i].mode)
				{
					case NG_PATCH_MODE_SET:
						val.v4 = privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_ADD:
						val.v4 += privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_SUB:
						val.v4 -= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_MUL:
						val.v4 *= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_DIV:
						val.v4 /= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_NEG:
						*((int32_t *) &val) = - *((int32_t *) &val);
						break;
					case NG_PATCH_MODE_AND:
						val.v4 &= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_OR:
						val.v4 |= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_XOR:
						val.v4 ^= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_SHL:
						val.v4 <<= privp->conf->ops[i].val.v4;
						break;
					case NG_PATCH_MODE_SHR:
						val.v4 >>= privp->conf->ops[i].val.v4;
						break;
				}

				val.v4 = htonl(val.v4);

				break;

			case 8:
				val.v8 = be64toh(val.v8);

				switch (privp->conf->ops[i].mode)
				{
					case NG_PATCH_MODE_SET:
						val.v8 = privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_ADD:
						val.v8 += privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_SUB:
						val.v8 -= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_MUL:
						val.v8 *= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_DIV:
						val.v8 /= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_NEG:
						*((int64_t *) &val) = - *((int64_t *) &val);
						break;
					case NG_PATCH_MODE_AND:
						val.v8 &= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_OR:
						val.v8 |= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_XOR:
						val.v8 ^= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_SHL:
						val.v8 <<= privp->conf->ops[i].val.v8;
						break;
					case NG_PATCH_MODE_SHR:
						val.v8 >>= privp->conf->ops[i].val.v8;
						break;
				}

				val.v8 = htobe64(val.v8);

				break;
		}

		m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
		patched = 1;
	}

	if (patched)
		privp->stats.patched++;
}

static int
ng_patch_rcvdata(hook_p hook, item_p item)
{
	const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
	struct mbuf *m;
	hook_p out;
	int pullup_len = 0;
	int error = 0;

	priv->stats.received++;

	NGI_GET_M(item, m);

#define	PULLUP_CHECK(mbuf, length) do {					\
	pullup_len += length;						\
	if (((mbuf)->m_pkthdr.len < pullup_len) ||			\
	    (pullup_len > MHLEN)) {					\
		error = EINVAL;						\
		goto bypass;						\
	}								\
	if ((mbuf)->m_len < pullup_len &&				\
	    (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) {	\
		error = ENOBUFS;					\
		goto drop;						\
	}								\
} while (0)

	if (priv->conf && hook == priv->in &&
	    m && (m->m_flags & M_PKTHDR)) {

		m = m_unshare(m, M_NOWAIT);

		if (m == NULL)
			ERROUT(ENOMEM);

		if (priv->conf->relative_offset) {
			struct ether_header *eh;
			struct ng_patch_vlan_header *vh;
			uint16_t etype;

			switch (priv->dlt)
			{
				case DLT_EN10MB:
					PULLUP_CHECK(m, sizeof(struct ether_header));
					eh = mtod(m, struct ether_header *);
					etype = ntohs(eh->ether_type);

					for (;;) {	/* QinQ support */
						switch (etype)
						{
							case 0x8100:
							case 0x88A8:
							case 0x9100:
								PULLUP_CHECK(m, sizeof(struct ng_patch_vlan_header));
								vh = (struct ng_patch_vlan_header *) mtodo(m,
								    pullup_len - sizeof(struct ng_patch_vlan_header));
								etype = ntohs(vh->etype);
								break;

							default:
								goto loopend;
						}
					}
loopend:
					break;

				case DLT_RAW:
					break;

				default:
					ERROUT(EINVAL);
			}
		}

		do_patch(priv, m, pullup_len);

		m->m_pkthdr.csum_flags |= priv->conf->csum_flags;
	}

#undef	PULLUP_CHECK

bypass:
	out = NULL;

	if (hook == priv->in) {
		/* return frames on 'in' hook if 'out' not connected */
		out = priv->out ? priv->out : priv->in;
	} else if (hook == priv->out && priv->in) {
		/* pass frames on 'out' hook if 'in' connected */
		out = priv->in;
	}

	if (out == NULL)
		ERROUT(0);

	NG_FWD_NEW_DATA(error, item, out, m);

	return (error);

done:
drop:
	NG_FREE_ITEM(item);
	NG_FREE_M(m);

	priv->stats.dropped++;

	return (error);
}

static int
ng_patch_shutdown(node_p node)
{
	const priv_p privdata = NG_NODE_PRIVATE(node);

	NG_NODE_SET_PRIVATE(node, NULL);
	NG_NODE_UNREF(node);

	if (privdata->conf != NULL)
		free(privdata->conf, M_NETGRAPH);

	free(privdata, M_NETGRAPH);

	return (0);
}

static int
ng_patch_disconnect(hook_p hook)
{
	priv_p priv;

	priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));

	if (hook == priv->in) {
		priv->in = NULL;
	}

	if (hook == priv->out) {
		priv->out = NULL;
	}

	if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 &&
	    NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */
		ng_rmnode_self(NG_HOOK_NODE(hook));

	return (0);
}