Changes to allow the patching of packets with an offset (and other changes.. see man page)

PR:	206185
Submitted by:	Dmitry Vagin <daemon-hammer@ya.ru>
MFC after:	 1 week
Relnotes:	yes (also ng_checksum node)

share/man/man4/ng_patch.4

@@ -1,5 +1,6 @@
 .\" Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com>
 .\" Copyright (c) 2010 Vadim Goncharov <vadimnuclight@tpu.ru>
+.\" Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru>
 .\" All rights reserved.
 .\"
 .\" Redistribution and use in source and binary forms, with or without
@@ -25,7 +26,7 @@
 .\"
 .\" $FreeBSD$
 .\"
-.Dd March 5, 2012
+.Dd November 17, 2015
 .Dt NG_PATCH 4
 .Os
 .Sh NAME
@@ -47,33 +48,52 @@ A negation operation is the one exception: integer is treated as signed
 and second operand (the
 .Va value )
 is not used.
-There may be several modification operations, they are all applied
+If there is more than one modification operation, they are applied
-to a packet sequentially in order they were specified by user.
+to packets sequentially in the order they were specified by the user.
-Data payload of packet is viewed as array of bytes, with zero offset
+The data payload of a packet is viewed as an array of bytes, with a zero offset
-corresponding to the very first byte of packet headers, and
+corresponding to the very first byte of packet headers, and the
 .Va length
 bytes beginning from
 .Va offset
-are taken as a single integer in network byte order.
+as a single integer in network byte order. An additional offset can be optionally 
+requested at configuration time to account for packet type.
 .Sh HOOKS
 This node type has two hooks:
 .Bl -tag -width ".Va out"
 .It Va in
 Packets received on this hook are modified according to rules specified
-in config and then forwarded to
+in the configuration and then forwarded to the
 .Ar out
-hook, if it exists and connected.
+hook, if it exists.
 Otherwise they are reflected back to the
 .Ar in
 hook.
 .It Va out
-Packets received on this hook are forwarded to
+Packets received on this hook are forwarded to the
 .Ar in
 hook without any changes.
 .El
 .Sh CONTROL MESSAGES
 This node type supports the generic control messages, plus the following:
 .Bl -tag -width foo
+.It Dv NGM_PATCH_SETDLT Pq Ic setdlt
+Sets the data link type on the
+.Va in
+hook (to help calculate relative offset). Currently, supported types are
+.Cm DLT_RAW
+(raw IP datagrams , no offset applied, the default) and
+.Cm DLT_EN10MB
+(Ethernet). DLT_ definitions can be found in
+.In net/bpf.h .
+If you want to work on the link layer header you must use no additional offset by specifying
+.Cm DLT_RAW .
+If
+.Cm EN10MB 
+is specified, then the optional additional offset will take into account the Ethernet header and a QinQ header if present.
+.It Dv NGM_PATCH_GETDLT Pq Ic getdlt
+This control message returns the data link type of the
+.Va in
+hook.
 .It Dv NGM_PATCH_SETCONFIG Pq Ic setconfig
 This command sets the sequence of modify operations
 that will be applied to incoming data on a hook.
@@ -82,10 +102,10 @@ The following
 must be supplied as an argument:
 .Bd -literal -offset 4n
 struct ng_patch_op {
-	uint64_t	value;
 	uint32_t	offset;
 	uint16_t	length; /* 1,2,4 or 8 bytes */
 	uint16_t	mode;
+	uint64_t	value;
 };
 /* Patching modes */
 #define NG_PATCH_MODE_SET	1
@@ -103,6 +123,7 @@ struct ng_patch_op {
 struct ng_patch_config {
 	uint32_t	count;
 	uint32_t	csum_flags;
+	uint32_t	relative_offset;
 	struct ng_patch_op ops[];
 };
 .Ed
@@ -116,14 +137,14 @@ The
 .Nm
 node does not do any checksum correction by itself.
 .It Dv NGM_PATCH_GETCONFIG Pq Ic getconfig
-This control message obtains current set of modify operations,
+This control message returns the current set of modify operations,
-returned as
+in the form of a
 .Vt "struct ng_patch_config" .
 .It Dv NGM_PATCH_GET_STATS Pq Ic getstats
-Returns node statistics as a
+Returns the node's statistics as a
 .Vt "struct ng_patch_stats" .
 .It Dv NGM_PATCH_CLR_STATS Pq Ic clrstats
-Clear node statistics.
+Clears the node's statistics.
 .It Dv NGM_PATCH_GETCLR_STATS Pq Ic getclrstats
 This command is identical to
 .Dv NGM_PATCH_GET_STATS ,
@@ -134,19 +155,20 @@ This node shuts down upon receipt of a
 .Dv NGM_SHUTDOWN
 control message, or when all hooks have been disconnected.
 .Sh EXAMPLES
-The
+This
 .Nm
-node allows to modify TTL and TOS/DSCP fields in IP packets.
+node was designed to modify TTL and TOS/DSCP fields in IP packets.
-Suppose you have two adjacent simplex links to remote network
+As an example,
+suppose you have two adjacent simplex links to a remote network
 (e.g.\& satellite), so that the packets expiring in between
 will generate unwanted ICMP-replies which have to go forth, not back.
 Thus you need to raise TTL of every packet entering link by 2
 to ensure the TTL will not reach zero there.
-So you setup
+To achieve this you can set an
 .Xr ipfw 8
-rule with
+rule to use the
 .Cm netgraph
-action to inject packets going to other end of simplex link by the
+action to inject packets which are going to the simplex link into the patch node, by using the
 following
 .Xr ngctl 8
 script:
@@ -160,11 +182,11 @@ SEQ
 /sbin/ipfw add 150 netgraph 200 ip from any to simplex.remote.net
 .Ed
 .Pp
-Here
+Here the
 .Dq Li ttl_add
 node of type
 .Nm
-configured to add (mode
+is configured to add (mode
 .Dv NG_PATCH_MODE_ADD )
 a
 .Va value
@@ -218,13 +240,15 @@ The
 node type was implemented in
 .Fx 8.1 .
 .Sh AUTHORS
-.An Maxim Ignatenko Aq Mt gelraen.ua@gmail.com
+.An "Maxim Ignatenko" Aq gelraen.ua@gmail.com .
+.Pp
+Relative offset code by
+.An "DMitry Vagin"
 .Pp
-.An -nosplit
 This manual page was written by
-.An Vadim Goncharov Aq Mt vadimnuclight@tpu.ru .
+.An "Vadim Goncharov" Aq vadimnuclight@tpu.ru .
 .Sh BUGS
-Node blindly tries to apply every patching operation to each packet
+The node blindly tries to apply every patching operation to each packet
 (except those which offset if greater than length of the packet),
 so be sure that you supply only the right packets to it (e.g. changing
 bytes in the ARP packets meant to be in IP header could corrupt
@@ -232,6 +256,6 @@ them and make your machine unreachable from the network).
 .Pp
 .Em !!! WARNING !!!
 .Pp
-Output path of the IP stack assumes correct fields and lengths in the
+The output path of the IP stack assumes correct fields and lengths in the
-packets - changing them by mistake to incorrect values can cause
+packets - changing them by to incorrect values can cause
 unpredictable results including kernel panics.

sys/netgraph/ng_patch.c

@@ -1,5 +1,6 @@
 /*-
  * 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
@@ -34,11 +35,28 @@ __FBSDID("$FreeBSD$");
 #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/ng_patch.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;
@@ -46,6 +64,8 @@ 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)
@@ -59,7 +79,7 @@ ng_patch_config_getlen(const struct ng_parse_type *type,
 }
 
 static const struct ng_parse_struct_field ng_patch_op_type_fields[]
-	= NG_PATCH_OP_TYPE_INFO;
+	= NG_PATCH_OP_TYPE;
 static const struct ng_parse_type ng_patch_op_type = {
 	&ng_parse_struct_type,
 	&ng_patch_op_type_fields
@@ -75,20 +95,34 @@ static const struct ng_parse_type ng_patch_ops_array_type = {
 };
 
 static const struct ng_parse_struct_field ng_patch_config_type_fields[]
-	= NG_PATCH_CONFIG_TYPE_INFO;
+	= 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_INFO;
+	= 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,
@@ -141,38 +175,16 @@ static struct ng_type typestruct = {
 
 NETGRAPH_INIT(patch, &typestruct);
 
-union patch_val {
-	uint8_t		v1;
-	uint16_t	v2;
-	uint32_t	v4;
-	uint64_t	v8;
-};
-
-/* private data */
-struct ng_patch_priv {
-	hook_p		in;
-	hook_p		out;
-	struct ng_patch_config *config;
-	union patch_val *val;
-	struct ng_patch_stats stats;
-};
-typedef struct ng_patch_priv *priv_p;
-
-#define	NG_PATCH_CONF_SIZE(count)	(sizeof(struct ng_patch_config) + \
-		(count) * sizeof(struct ng_patch_op))
-
-static void do_patch(priv_p conf, struct mbuf *m);
-
 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);
-	privdata->in = NULL;
+
-	privdata->out = NULL;
-	privdata->config = NULL;
 	return (0);
 }
 
@@ -188,6 +200,7 @@ ng_patch_newhook(node_p node, hook_p hook, const char *name)
 		privp->out = hook;
 	} else
 		return (EINVAL);
+
 	return (0);
 }
 
@@ -196,308 +209,341 @@ 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;
-	union patch_val *newval;
 	struct ng_mesg *msg;
-	struct ng_mesg *resp;
+	struct ng_mesg *resp = NULL;
-	int i, clear, error;
+	int i, error = 0;
 
-	clear = error = 0;
-	resp = NULL;
 	NGI_GET_MSG(item, msg);
-	switch (msg->header.typecookie) {
+
-	case NGM_PATCH_COOKIE:
+	if  (msg->header.typecookie != NGM_PATCH_COOKIE)
-		switch (msg->header.cmd) {
+		ERROUT(EINVAL);
-		case NGM_PATCH_GETCONFIG:
+
-			if (privp->config == NULL)
+	switch (msg->header.cmd)
-				break;
-			NG_MKRESPONSE(resp, msg,
-			    NG_PATCH_CONF_SIZE(privp->config->count),
-			    M_WAITOK);
-			bcopy(privp->config, resp->data,
-			    NG_PATCH_CONF_SIZE(privp->config->count));
-			break;
-		case NGM_PATCH_SETCONFIG:
 	{
-			if (msg->header.arglen <
+		case NGM_PATCH_GETCONFIG:
-			    sizeof(struct ng_patch_config)) {
+			if (privp->conf == NULL)
-				error = EINVAL;
+				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 <
+
-			    NG_PATCH_CONF_SIZE(conf->count)) {
+			if (msg->header.arglen < sizeof(struct ng_patch_config) ||
-				error = EINVAL;
+			    msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count))
-				break;
+				ERROUT(EINVAL);
-			}
 
 			for (i = 0; i < conf->count; i++) {
-				switch(conf->ops[i].length) {
+				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:
-					error = EINVAL;
+						ERROUT(EINVAL);
-					break;
 				}
-				if (error != 0)
-					break;
 			}
 
-			conf->csum_flags &= CSUM_IP | CSUM_TCP | CSUM_UDP |
+			conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6;
-			    CSUM_SCTP;
+			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;
 
-			if (error == 0) {
-				newconf = malloc(
-				    NG_PATCH_CONF_SIZE(conf->count),
-				    M_NETGRAPH, M_WAITOK);
-				newval = malloc(conf->count *
-				    sizeof(union patch_val), M_NETGRAPH,
-				    M_WAITOK);
-				for(i = 0; i < conf->count; i++) {
-					switch (conf->ops[i].length) {
-					case 1:
-						newval[i].v1 =
-						    conf->ops[i].value;
 			break;
-					case 2:
+
-						newval[i].v2 =
-						    conf->ops[i].value;
-						break;
-					case 4:
-						newval[i].v4 =
-						    conf->ops[i].value;
-						break;
-					case 8:
-						newval[i].v8 =
-						    conf->ops[i].value;
-						break;
-					}
-				}
-				bcopy(conf, newconf,
-				    NG_PATCH_CONF_SIZE(conf->count));
-				if (privp->val != NULL)
-					free(privp->val, M_NETGRAPH);
-				privp->val = newval;
-				if (privp->config != NULL)
-					free(privp->config, M_NETGRAPH);
-				privp->config = newconf;
-			}
-			break;
-		    }
-		case NGM_PATCH_GETCLR_STATS:
-			clear = 1;
-			/* FALLTHROUGH */
 		case NGM_PATCH_GET_STATS:
-			NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats),
-			    M_WAITOK);
-			bcopy(&(privp->stats), resp->data,
-			    sizeof(struct ng_patch_stats));
-			if (clear == 0)
-				break;
-			/* else FALLTHROUGH */
 		case NGM_PATCH_CLR_STATS:
-			bzero(&(privp->stats), sizeof(struct ng_patch_stats));
+		case NGM_PATCH_GETCLR_STATS:
-			break;
+			if (msg->header.cmd != NGM_PATCH_CLR_STATS) {
-		default:
+				NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK);
-			error = EINVAL;
+
-			break;
+				if (resp == NULL)
-		}
+					ERROUT(ENOMEM);
-		break;
+
-	default:
+				bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats));
-		error = EINVAL;
-		break;
 			}
 
+			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)
+do_patch(priv_p privp, struct mbuf *m, int global_offset)
 {
-	struct ng_patch_config *conf;
+	int i, offset, patched = 0;
-	uint64_t buf;
+	union ng_patch_op_val val;
-	int i, patched;
 
-	conf = privp->config;
+	for (i = 0; i < privp->conf->count; i++) {
-	patched = 0;
+		offset = global_offset + privp->conf->ops[i].offset;
-	for(i = 0; i < conf->count; i++) {
+
-		if (conf->ops[i].offset + conf->ops[i].length >
+		if (offset + privp->conf->ops[i].length > m->m_pkthdr.len)
-		    m->m_pkthdr.len)
 			continue;
 
 		/* for "=" operation we don't need to copy data from mbuf */
-		if (conf->ops[i].mode != NG_PATCH_MODE_SET) {
+		if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET)
-			m_copydata(m, conf->ops[i].offset,
+			m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
-			    conf->ops[i].length, (caddr_t)&buf);
-		}
 
-		switch (conf->ops[i].length) {
+		switch (privp->conf->ops[i].length)
+		{
 			case 1:
-			switch (conf->ops[i].mode) {
+				switch (privp->conf->ops[i].mode)
+				{
 					case NG_PATCH_MODE_SET:
-				*((uint8_t *)&buf) = privp->val[i].v1;
+						val.v1 = privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_ADD:
-				*((uint8_t *)&buf) += privp->val[i].v1;
+						val.v1 += privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_SUB:
-				*((uint8_t *)&buf) -= privp->val[i].v1;
+						val.v1 -= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_MUL:
-				*((uint8_t *)&buf) *= privp->val[i].v1;
+						val.v1 *= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_DIV:
-				*((uint8_t *)&buf) /= privp->val[i].v1;
+						val.v1 /= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_NEG:
-				*((int8_t *)&buf) = - *((int8_t *)&buf);
+						*((int8_t *) &val) = - *((int8_t *) &val);
 						break;
 					case NG_PATCH_MODE_AND:
-				*((uint8_t *)&buf) &= privp->val[i].v1;
+						val.v1 &= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_OR:
-				*((uint8_t *)&buf) |= privp->val[i].v1;
+						val.v1 |= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_XOR:
-				*((uint8_t *)&buf) ^= privp->val[i].v1;
+						val.v1 ^= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_SHL:
-				*((uint8_t *)&buf) <<= privp->val[i].v1;
+						val.v1 <<= privp->conf->ops[i].val.v1;
 						break;
 					case NG_PATCH_MODE_SHR:
-				*((uint8_t *)&buf) >>= privp->val[i].v1;
+						val.v1 >>= privp->conf->ops[i].val.v1;
 						break;
 				}
 				break;
+
 			case 2:
-			*((int16_t *)&buf) =  ntohs(*((int16_t *)&buf));
+				val.v2 = ntohs(val.v2);
-			switch (conf->ops[i].mode) {
+
+				switch (privp->conf->ops[i].mode)
+				{
 					case NG_PATCH_MODE_SET:
-				*((uint16_t *)&buf) = privp->val[i].v2;
+						val.v2 = privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_ADD:
-				*((uint16_t *)&buf) += privp->val[i].v2;
+						val.v2 += privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_SUB:
-				*((uint16_t *)&buf) -= privp->val[i].v2;
+						val.v2 -= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_MUL:
-				*((uint16_t *)&buf) *= privp->val[i].v2;
+						val.v2 *= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_DIV:
-				*((uint16_t *)&buf) /= privp->val[i].v2;
+						val.v2 /= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_NEG:
-				*((int16_t *)&buf) = - *((int16_t *)&buf);
+						*((int16_t *) &val) = - *((int16_t *) &val);
 						break;
 					case NG_PATCH_MODE_AND:
-				*((uint16_t *)&buf) &= privp->val[i].v2;
+						val.v2 &= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_OR:
-				*((uint16_t *)&buf) |= privp->val[i].v2;
+						val.v2 |= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_XOR:
-				*((uint16_t *)&buf) ^= privp->val[i].v2;
+						val.v2 ^= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_SHL:
-				*((uint16_t *)&buf) <<= privp->val[i].v2;
+						val.v2 <<= privp->conf->ops[i].val.v2;
 						break;
 					case NG_PATCH_MODE_SHR:
-				*((uint16_t *)&buf) >>= privp->val[i].v2;
+						val.v2 >>= privp->conf->ops[i].val.v2;
-				break;
-			}
-			*((int16_t *)&buf) =  htons(*((int16_t *)&buf));
-			break;
-		case 4:
-			*((int32_t *)&buf) =  ntohl(*((int32_t *)&buf));
-			switch (conf->ops[i].mode) {
-			case NG_PATCH_MODE_SET:
-				*((uint32_t *)&buf) = privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_ADD:
-				*((uint32_t *)&buf) += privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_SUB:
-				*((uint32_t *)&buf) -= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_MUL:
-				*((uint32_t *)&buf) *= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_DIV:
-				*((uint32_t *)&buf) /= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_NEG:
-				*((int32_t *)&buf) = - *((int32_t *)&buf);
-				break;
-			case NG_PATCH_MODE_AND:
-				*((uint32_t *)&buf) &= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_OR:
-				*((uint32_t *)&buf) |= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_XOR:
-				*((uint32_t *)&buf) ^= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_SHL:
-				*((uint32_t *)&buf) <<= privp->val[i].v4;
-				break;
-			case NG_PATCH_MODE_SHR:
-				*((uint32_t *)&buf) >>= privp->val[i].v4;
-				break;
-			}
-			*((int32_t *)&buf) =  htonl(*((int32_t *)&buf));
-			break;
-		case 8:
-			*((int64_t *)&buf) =  be64toh(*((int64_t *)&buf));
-			switch (conf->ops[i].mode) {
-			case NG_PATCH_MODE_SET:
-				*((uint64_t *)&buf) = privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_ADD:
-				*((uint64_t *)&buf) += privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_SUB:
-				*((uint64_t *)&buf) -= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_MUL:
-				*((uint64_t *)&buf) *= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_DIV:
-				*((uint64_t *)&buf) /= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_NEG:
-				*((int64_t *)&buf) = - *((int64_t *)&buf);
-				break;
-			case NG_PATCH_MODE_AND:
-				*((uint64_t *)&buf) &= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_OR:
-				*((uint64_t *)&buf) |= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_XOR:
-				*((uint64_t *)&buf) ^= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_SHL:
-				*((uint64_t *)&buf) <<= privp->val[i].v8;
-				break;
-			case NG_PATCH_MODE_SHR:
-				*((uint64_t *)&buf) >>= privp->val[i].v8;
-				break;
-			}
-			*((int64_t *)&buf) =  htobe64(*((int64_t *)&buf));
 						break;
 				}
 
-		m_copyback(m, conf->ops[i].offset, conf->ops[i].length,
+				val.v2 = htons(val.v2);
-		    (caddr_t)&buf);
+
+				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 > 0)
+
+	if (patched)
 		privp->stats.patched++;
 }
 
@@ -506,41 +552,107 @@ 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 target;
+	hook_p out;
-	int error;
+	int pullup_len = 0;
+	int error = 0;
 
 	priv->stats.received++;
+
 	NGI_GET_M(item, m);
-	if (priv->config != NULL && hook == priv->in &&
+
-	    (m->m_flags & M_PKTHDR) != 0) {
+#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) {
+
-			priv->stats.dropped++;
+		if (m == NULL)
-			NG_FREE_ITEM(item);
+			ERROUT(ENOMEM);
-			return (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);
-		m->m_pkthdr.csum_flags |= priv->config->csum_flags;
 		}
 
-	target = NULL;
+		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 */
-		if (priv->out != NULL)
+		out = priv->out ? priv->out : priv->in;
-			target = priv->out;
+	} else if (hook == priv->out && priv->in) {
-		else
+		/* pass frames on 'out' hook if 'in' connected */
-			target = priv->in;
+		out = priv->in;
 	}
-	if (hook == priv->out && priv->in != NULL)
-		target = priv->in;
 
-	if (target == NULL) {
+	if (out == NULL)
-		priv->stats.dropped++;
+		ERROUT(0);
+
+	NG_FWD_NEW_DATA(error, item, out, m);
+
+	return (error);
+
+done:
+drop:
 	NG_FREE_ITEM(item);
 	NG_FREE_M(m);
-		return (0);
+
-	}
+	priv->stats.dropped++;
-	NG_FWD_NEW_DATA(error, item, target, m);
+
 	return (error);
 }
 
@@ -549,13 +661,14 @@ ng_patch_shutdown(node_p node)
 {
 	const priv_p privdata = NG_NODE_PRIVATE(node);
 
-	if (privdata->val != NULL)
-		free(privdata->val, M_NETGRAPH);
-	if (privdata->config != NULL)
-		free(privdata->config, M_NETGRAPH);
 	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);
 }
 
@@ -565,15 +678,18 @@ 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);
 }
-

sys/netgraph/ng_patch.h

@@ -1,5 +1,6 @@
 /*-
- * Copyright (C) 2010 by Maxim Ignatenko <gelraen.ua@gmail.com>
+ * 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
@@ -39,13 +40,19 @@
 #define	NG_PATCH_HOOK_IN	"in"
 #define	NG_PATCH_HOOK_OUT	"out"
 
+/* Checksum flags */
+#define NG_PATCH_CSUM_IPV4	(CSUM_IP|CSUM_TCP|CSUM_UDP|CSUM_SCTP)
+#define NG_PATCH_CSUM_IPV6	(CSUM_TCP_IPV6|CSUM_UDP_IPV6|CSUM_SCTP_IPV6)
+
 /* Netgraph commands understood by this node type */
 enum {
 	NGM_PATCH_SETCONFIG = 1,
 	NGM_PATCH_GETCONFIG,
 	NGM_PATCH_GET_STATS,
 	NGM_PATCH_CLR_STATS,
-	NGM_PATCH_GETCLR_STATS
+	NGM_PATCH_GETCLR_STATS,
+	NGM_PATCH_GETDLT,
+	NGM_PATCH_SETDLT
 };
 
 /* Patching modes */
@@ -63,45 +70,64 @@ enum {
 	NG_PATCH_MODE_SHR = 11
 };
 
-struct ng_patch_op {
+/* Parsing declarations */
-	uint64_t	value;
-	uint32_t	offset;
-	uint16_t	length;	/* 1, 2, 4 or 8 (bytes) */
-	uint16_t	mode;
-};
 
-#define	NG_PATCH_OP_TYPE_INFO {				\
+#define	NG_PATCH_CONFIG_TYPE {						\
-	{ "value",	&ng_parse_uint64_type	},	\
-	{ "offset",	&ng_parse_uint32_type	},	\
-	{ "length",	&ng_parse_uint16_type	},	\
-	{ "mode",	&ng_parse_uint16_type	},	\
-	{ NULL }					\
-}
-
-struct ng_patch_config {
-	uint32_t	count;
-	uint32_t	csum_flags;
-	struct ng_patch_op ops[];
-};
-
-#define	NG_PATCH_CONFIG_TYPE_INFO {					\
 	{ "count",		&ng_parse_uint32_type		},	\
 	{ "csum_flags",		&ng_parse_uint64_type		},	\
+	{ "relative_offset",	&ng_parse_uint32_type		},	\
 	{ "ops",		&ng_patch_ops_array_type	},	\
 	{ NULL }							\
 }
 
+#define	NG_PATCH_OP_TYPE {				\
+	{ "offset",	&ng_parse_uint32_type	},	\
+	{ "length",	&ng_parse_uint16_type	},	\
+	{ "mode",	&ng_parse_uint16_type	},	\
+	{ "value",	&ng_parse_uint64_type	},	\
+	{ NULL }					\
+}
+
+#define	NG_PATCH_STATS_TYPE {				\
+	{ "Received",	&ng_parse_uint64_type	},	\
+	{ "Patched",	&ng_parse_uint64_type	},	\
+	{ "Dropped",	&ng_parse_uint64_type	},	\
+	{ NULL }					\
+}
+
+union ng_patch_op_val {
+	uint8_t		v1;
+	uint16_t	v2;
+	uint32_t	v4;
+	uint64_t	v8;
+};
+
+struct ng_patch_op {
+	uint32_t	offset;
+	uint16_t	length;	/* 1, 2, 4 or 8 (bytes) */
+	uint16_t	mode;
+	union ng_patch_op_val val;
+};
+
+struct ng_patch_config {
+	uint32_t	count;
+	uint64_t	csum_flags;
+	uint32_t	relative_offset;
+	struct ng_patch_op ops[];
+};
+
 struct ng_patch_stats {
 	uint64_t	received;
 	uint64_t	patched;
 	uint64_t	dropped;
 };
 
-#define	NG_PATCH_STATS_TYPE_INFO {			\
+struct ng_patch_vlan_header {
-	{ "Received",	&ng_parse_uint64_type	},	\
+	u_int16_t tag;
-	{ "Patched",	&ng_parse_uint64_type	},	\
+	u_int16_t etype;
-	{ "Dropped",	&ng_parse_uint64_type	},	\
+};
-	{ NULL }					\
+
-}
+#define NG_PATCH_CONF_SIZE(count) (sizeof(struct ng_patch_config) + \
+	(count) * sizeof(struct ng_patch_op))
 
 #endif /* _NETGRAPH_NG_PATCH_H_ */