/*- * SPDX-License-Identifier: BSD-2-Clause * * 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> #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); }