2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
4 * Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com>
5 * Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru>
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 #include <sys/cdefs.h>
32 __FBSDID("$FreeBSD$");
34 #include <sys/param.h>
35 #include <sys/systm.h>
36 #include <sys/kernel.h>
37 #include <sys/endian.h>
38 #include <sys/malloc.h>
42 #include <net/ethernet.h>
44 #include <netgraph/ng_message.h>
45 #include <netgraph/ng_parse.h>
46 #include <netgraph/netgraph.h>
48 #include <netgraph/ng_patch.h>
51 struct ng_patch_priv {
54 uint8_t dlt; /* DLT_XXX from bpf.h */
55 struct ng_patch_stats stats;
56 struct ng_patch_config *conf;
59 typedef struct ng_patch_priv *priv_p;
61 /* Netgraph methods */
62 static ng_constructor_t ng_patch_constructor;
63 static ng_rcvmsg_t ng_patch_rcvmsg;
64 static ng_shutdown_t ng_patch_shutdown;
65 static ng_newhook_t ng_patch_newhook;
66 static ng_rcvdata_t ng_patch_rcvdata;
67 static ng_disconnect_t ng_patch_disconnect;
68 #define ERROUT(x) { error = (x); goto done; }
71 ng_patch_config_getlen(const struct ng_parse_type *type,
72 const u_char *start, const u_char *buf)
74 const struct ng_patch_config *conf;
76 conf = (const struct ng_patch_config *)(buf -
77 offsetof(struct ng_patch_config, ops));
82 static const struct ng_parse_struct_field ng_patch_op_type_fields[]
84 static const struct ng_parse_type ng_patch_op_type = {
85 &ng_parse_struct_type,
86 &ng_patch_op_type_fields
89 static const struct ng_parse_array_info ng_patch_ops_array_info = {
91 &ng_patch_config_getlen
93 static const struct ng_parse_type ng_patch_ops_array_type = {
95 &ng_patch_ops_array_info
98 static const struct ng_parse_struct_field ng_patch_config_type_fields[]
99 = NG_PATCH_CONFIG_TYPE;
100 static const struct ng_parse_type ng_patch_config_type = {
101 &ng_parse_struct_type,
102 &ng_patch_config_type_fields
105 static const struct ng_parse_struct_field ng_patch_stats_fields[]
106 = NG_PATCH_STATS_TYPE;
107 static const struct ng_parse_type ng_patch_stats_type = {
108 &ng_parse_struct_type,
109 &ng_patch_stats_fields
112 static const struct ng_cmdlist ng_patch_cmdlist[] = {
124 &ng_parse_uint8_type,
132 &ng_patch_config_type
138 &ng_patch_config_type,
157 NGM_PATCH_GETCLR_STATS,
165 static struct ng_type typestruct = {
166 .version = NG_ABI_VERSION,
167 .name = NG_PATCH_NODE_TYPE,
168 .constructor = ng_patch_constructor,
169 .rcvmsg = ng_patch_rcvmsg,
170 .shutdown = ng_patch_shutdown,
171 .newhook = ng_patch_newhook,
172 .rcvdata = ng_patch_rcvdata,
173 .disconnect = ng_patch_disconnect,
174 .cmdlist = ng_patch_cmdlist,
177 NETGRAPH_INIT(patch, &typestruct);
180 ng_patch_constructor(node_p node)
184 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO);
185 privdata->dlt = DLT_RAW;
187 NG_NODE_SET_PRIVATE(node, privdata);
193 ng_patch_newhook(node_p node, hook_p hook, const char *name)
195 const priv_p privp = NG_NODE_PRIVATE(node);
197 if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) {
199 } else if (strncmp(name, NG_PATCH_HOOK_OUT,
200 strlen(NG_PATCH_HOOK_OUT)) == 0) {
209 ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook)
211 const priv_p privp = NG_NODE_PRIVATE(node);
212 struct ng_patch_config *conf, *newconf;
214 struct ng_mesg *resp = NULL;
217 NGI_GET_MSG(item, msg);
219 if (msg->header.typecookie != NGM_PATCH_COOKIE)
222 switch (msg->header.cmd)
224 case NGM_PATCH_GETCONFIG:
225 if (privp->conf == NULL)
228 NG_MKRESPONSE(resp, msg,
229 NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK);
234 bcopy(privp->conf, resp->data,
235 NG_PATCH_CONF_SIZE(privp->conf->count));
237 conf = (struct ng_patch_config *) resp->data;
239 for (i = 0; i < conf->count; i++) {
240 switch (conf->ops[i].length)
243 conf->ops[i].val.v8 = conf->ops[i].val.v1;
246 conf->ops[i].val.v8 = conf->ops[i].val.v2;
249 conf->ops[i].val.v8 = conf->ops[i].val.v4;
258 case NGM_PATCH_SETCONFIG:
259 conf = (struct ng_patch_config *) msg->data;
261 if (msg->header.arglen < sizeof(struct ng_patch_config) ||
262 msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count))
265 for (i = 0; i < conf->count; i++) {
266 switch (conf->ops[i].length)
269 conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8;
272 conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8;
275 conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8;
284 conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6;
285 conf->relative_offset = !!conf->relative_offset;
287 newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO);
289 bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count));
292 free(privp->conf, M_NETGRAPH);
294 privp->conf = newconf;
298 case NGM_PATCH_GET_STATS:
299 case NGM_PATCH_CLR_STATS:
300 case NGM_PATCH_GETCLR_STATS:
301 if (msg->header.cmd != NGM_PATCH_CLR_STATS) {
302 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK);
307 bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats));
310 if (msg->header.cmd != NGM_PATCH_GET_STATS)
311 bzero(&(privp->stats), sizeof(struct ng_patch_stats));
315 case NGM_PATCH_GETDLT:
316 NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK);
321 *((uint8_t *) resp->data) = privp->dlt;
325 case NGM_PATCH_SETDLT:
326 if (msg->header.arglen != sizeof(uint8_t))
329 switch (*(uint8_t *) msg->data)
333 privp->dlt = *(uint8_t *) msg->data;
347 NG_RESPOND_MSG(error, node, item, resp);
354 do_patch(priv_p privp, struct mbuf *m, int global_offset)
356 int i, offset, patched = 0;
357 union ng_patch_op_val val;
359 for (i = 0; i < privp->conf->count; i++) {
360 offset = global_offset + privp->conf->ops[i].offset;
362 if (offset + privp->conf->ops[i].length > m->m_pkthdr.len)
365 /* for "=" operation we don't need to copy data from mbuf */
366 if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET)
367 m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
369 switch (privp->conf->ops[i].length)
372 switch (privp->conf->ops[i].mode)
374 case NG_PATCH_MODE_SET:
375 val.v1 = privp->conf->ops[i].val.v1;
377 case NG_PATCH_MODE_ADD:
378 val.v1 += privp->conf->ops[i].val.v1;
380 case NG_PATCH_MODE_SUB:
381 val.v1 -= privp->conf->ops[i].val.v1;
383 case NG_PATCH_MODE_MUL:
384 val.v1 *= privp->conf->ops[i].val.v1;
386 case NG_PATCH_MODE_DIV:
387 val.v1 /= privp->conf->ops[i].val.v1;
389 case NG_PATCH_MODE_NEG:
390 *((int8_t *) &val) = - *((int8_t *) &val);
392 case NG_PATCH_MODE_AND:
393 val.v1 &= privp->conf->ops[i].val.v1;
395 case NG_PATCH_MODE_OR:
396 val.v1 |= privp->conf->ops[i].val.v1;
398 case NG_PATCH_MODE_XOR:
399 val.v1 ^= privp->conf->ops[i].val.v1;
401 case NG_PATCH_MODE_SHL:
402 val.v1 <<= privp->conf->ops[i].val.v1;
404 case NG_PATCH_MODE_SHR:
405 val.v1 >>= privp->conf->ops[i].val.v1;
411 val.v2 = ntohs(val.v2);
413 switch (privp->conf->ops[i].mode)
415 case NG_PATCH_MODE_SET:
416 val.v2 = privp->conf->ops[i].val.v2;
418 case NG_PATCH_MODE_ADD:
419 val.v2 += privp->conf->ops[i].val.v2;
421 case NG_PATCH_MODE_SUB:
422 val.v2 -= privp->conf->ops[i].val.v2;
424 case NG_PATCH_MODE_MUL:
425 val.v2 *= privp->conf->ops[i].val.v2;
427 case NG_PATCH_MODE_DIV:
428 val.v2 /= privp->conf->ops[i].val.v2;
430 case NG_PATCH_MODE_NEG:
431 *((int16_t *) &val) = - *((int16_t *) &val);
433 case NG_PATCH_MODE_AND:
434 val.v2 &= privp->conf->ops[i].val.v2;
436 case NG_PATCH_MODE_OR:
437 val.v2 |= privp->conf->ops[i].val.v2;
439 case NG_PATCH_MODE_XOR:
440 val.v2 ^= privp->conf->ops[i].val.v2;
442 case NG_PATCH_MODE_SHL:
443 val.v2 <<= privp->conf->ops[i].val.v2;
445 case NG_PATCH_MODE_SHR:
446 val.v2 >>= privp->conf->ops[i].val.v2;
450 val.v2 = htons(val.v2);
455 val.v4 = ntohl(val.v4);
457 switch (privp->conf->ops[i].mode)
459 case NG_PATCH_MODE_SET:
460 val.v4 = privp->conf->ops[i].val.v4;
462 case NG_PATCH_MODE_ADD:
463 val.v4 += privp->conf->ops[i].val.v4;
465 case NG_PATCH_MODE_SUB:
466 val.v4 -= privp->conf->ops[i].val.v4;
468 case NG_PATCH_MODE_MUL:
469 val.v4 *= privp->conf->ops[i].val.v4;
471 case NG_PATCH_MODE_DIV:
472 val.v4 /= privp->conf->ops[i].val.v4;
474 case NG_PATCH_MODE_NEG:
475 *((int32_t *) &val) = - *((int32_t *) &val);
477 case NG_PATCH_MODE_AND:
478 val.v4 &= privp->conf->ops[i].val.v4;
480 case NG_PATCH_MODE_OR:
481 val.v4 |= privp->conf->ops[i].val.v4;
483 case NG_PATCH_MODE_XOR:
484 val.v4 ^= privp->conf->ops[i].val.v4;
486 case NG_PATCH_MODE_SHL:
487 val.v4 <<= privp->conf->ops[i].val.v4;
489 case NG_PATCH_MODE_SHR:
490 val.v4 >>= privp->conf->ops[i].val.v4;
494 val.v4 = htonl(val.v4);
499 val.v8 = be64toh(val.v8);
501 switch (privp->conf->ops[i].mode)
503 case NG_PATCH_MODE_SET:
504 val.v8 = privp->conf->ops[i].val.v8;
506 case NG_PATCH_MODE_ADD:
507 val.v8 += privp->conf->ops[i].val.v8;
509 case NG_PATCH_MODE_SUB:
510 val.v8 -= privp->conf->ops[i].val.v8;
512 case NG_PATCH_MODE_MUL:
513 val.v8 *= privp->conf->ops[i].val.v8;
515 case NG_PATCH_MODE_DIV:
516 val.v8 /= privp->conf->ops[i].val.v8;
518 case NG_PATCH_MODE_NEG:
519 *((int64_t *) &val) = - *((int64_t *) &val);
521 case NG_PATCH_MODE_AND:
522 val.v8 &= privp->conf->ops[i].val.v8;
524 case NG_PATCH_MODE_OR:
525 val.v8 |= privp->conf->ops[i].val.v8;
527 case NG_PATCH_MODE_XOR:
528 val.v8 ^= privp->conf->ops[i].val.v8;
530 case NG_PATCH_MODE_SHL:
531 val.v8 <<= privp->conf->ops[i].val.v8;
533 case NG_PATCH_MODE_SHR:
534 val.v8 >>= privp->conf->ops[i].val.v8;
538 val.v8 = htobe64(val.v8);
543 m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
548 privp->stats.patched++;
552 ng_patch_rcvdata(hook_p hook, item_p item)
554 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
560 priv->stats.received++;
564 #define PULLUP_CHECK(mbuf, length) do { \
565 pullup_len += length; \
566 if (((mbuf)->m_pkthdr.len < pullup_len) || \
567 (pullup_len > MHLEN)) { \
571 if ((mbuf)->m_len < pullup_len && \
572 (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) { \
578 if (priv->conf && hook == priv->in &&
579 m && (m->m_flags & M_PKTHDR)) {
580 m = m_unshare(m, M_NOWAIT);
585 if (priv->conf->relative_offset) {
586 struct ether_header *eh;
587 struct ng_patch_vlan_header *vh;
593 PULLUP_CHECK(m, sizeof(struct ether_header));
594 eh = mtod(m, struct ether_header *);
595 etype = ntohs(eh->ether_type);
597 for (;;) { /* QinQ support */
603 PULLUP_CHECK(m, sizeof(struct ng_patch_vlan_header));
604 vh = (struct ng_patch_vlan_header *) mtodo(m,
605 pullup_len - sizeof(struct ng_patch_vlan_header));
606 etype = ntohs(vh->etype);
624 do_patch(priv, m, pullup_len);
626 m->m_pkthdr.csum_flags |= priv->conf->csum_flags;
634 if (hook == priv->in) {
635 /* return frames on 'in' hook if 'out' not connected */
636 out = priv->out ? priv->out : priv->in;
637 } else if (hook == priv->out && priv->in) {
638 /* pass frames on 'out' hook if 'in' connected */
645 NG_FWD_NEW_DATA(error, item, out, m);
654 priv->stats.dropped++;
660 ng_patch_shutdown(node_p node)
662 const priv_p privdata = NG_NODE_PRIVATE(node);
664 NG_NODE_SET_PRIVATE(node, NULL);
667 if (privdata->conf != NULL)
668 free(privdata->conf, M_NETGRAPH);
670 free(privdata, M_NETGRAPH);
676 ng_patch_disconnect(hook_p hook)
680 priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
682 if (hook == priv->in) {
686 if (hook == priv->out) {
690 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 &&
691 NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */
692 ng_rmnode_self(NG_HOOK_NODE(hook));