1/*- 2 * SPDX-License-Identifier: BSD-2-Clause 3 * 4 * Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com> 5 * Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru> 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 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. 16 * 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 27 * SUCH DAMAGE. 28 * 29 */ 30 31#include <sys/param.h> 32#include <sys/systm.h> 33#include <sys/kernel.h> 34#include <sys/endian.h> 35#include <sys/malloc.h> 36#include <sys/mbuf.h> 37 38#include <net/bpf.h> 39#include <net/ethernet.h> 40 41#include <netgraph/ng_message.h> 42#include <netgraph/ng_parse.h> 43#include <netgraph/netgraph.h> 44 45#include <netgraph/ng_patch.h> 46 47/* private data */ 48struct ng_patch_priv { 49 hook_p in; 50 hook_p out; 51 uint8_t dlt; /* DLT_XXX from bpf.h */ 52 struct ng_patch_stats stats; 53 struct ng_patch_config *conf; 54}; 55 56typedef struct ng_patch_priv *priv_p; 57 58/* Netgraph methods */ 59static ng_constructor_t ng_patch_constructor; 60static ng_rcvmsg_t ng_patch_rcvmsg; 61static ng_shutdown_t ng_patch_shutdown; 62static ng_newhook_t ng_patch_newhook; 63static ng_rcvdata_t ng_patch_rcvdata; 64static ng_disconnect_t ng_patch_disconnect; 65#define ERROUT(x) { error = (x); goto done; } 66 67static int 68ng_patch_config_getlen(const struct ng_parse_type *type, 69 const u_char *start, const u_char *buf) 70{ 71 const struct ng_patch_config *conf; 72 73 conf = (const struct ng_patch_config *)(buf - 74 offsetof(struct ng_patch_config, ops)); 75 76 return (conf->count); 77} 78 79static const struct ng_parse_struct_field ng_patch_op_type_fields[] 80 = NG_PATCH_OP_TYPE; 81static const struct ng_parse_type ng_patch_op_type = { 82 &ng_parse_struct_type, 83 &ng_patch_op_type_fields 84}; 85 86static const struct ng_parse_array_info ng_patch_ops_array_info = { 87 &ng_patch_op_type, 88 &ng_patch_config_getlen 89}; 90static const struct ng_parse_type ng_patch_ops_array_type = { 91 &ng_parse_array_type, 92 &ng_patch_ops_array_info 93}; 94 95static const struct ng_parse_struct_field ng_patch_config_type_fields[] 96 = NG_PATCH_CONFIG_TYPE; 97static const struct ng_parse_type ng_patch_config_type = { 98 &ng_parse_struct_type, 99 &ng_patch_config_type_fields 100}; 101 102static const struct ng_parse_struct_field ng_patch_stats_fields[] 103 = NG_PATCH_STATS_TYPE; 104static const struct ng_parse_type ng_patch_stats_type = { 105 &ng_parse_struct_type, 106 &ng_patch_stats_fields 107}; 108 109static const struct ng_cmdlist ng_patch_cmdlist[] = { 110 { 111 NGM_PATCH_COOKIE, 112 NGM_PATCH_GETDLT, 113 "getdlt", 114 NULL, 115 &ng_parse_uint8_type 116 }, 117 { 118 NGM_PATCH_COOKIE, 119 NGM_PATCH_SETDLT, 120 "setdlt", 121 &ng_parse_uint8_type, 122 NULL 123 }, 124 { 125 NGM_PATCH_COOKIE, 126 NGM_PATCH_GETCONFIG, 127 "getconfig", 128 NULL, 129 &ng_patch_config_type 130 }, 131 { 132 NGM_PATCH_COOKIE, 133 NGM_PATCH_SETCONFIG, 134 "setconfig", 135 &ng_patch_config_type, 136 NULL 137 }, 138 { 139 NGM_PATCH_COOKIE, 140 NGM_PATCH_GET_STATS, 141 "getstats", 142 NULL, 143 &ng_patch_stats_type 144 }, 145 { 146 NGM_PATCH_COOKIE, 147 NGM_PATCH_CLR_STATS, 148 "clrstats", 149 NULL, 150 NULL 151 }, 152 { 153 NGM_PATCH_COOKIE, 154 NGM_PATCH_GETCLR_STATS, 155 "getclrstats", 156 NULL, 157 &ng_patch_stats_type 158 }, 159 { 0 } 160}; 161 162static struct ng_type typestruct = { 163 .version = NG_ABI_VERSION, 164 .name = NG_PATCH_NODE_TYPE, 165 .constructor = ng_patch_constructor, 166 .rcvmsg = ng_patch_rcvmsg, 167 .shutdown = ng_patch_shutdown, 168 .newhook = ng_patch_newhook, 169 .rcvdata = ng_patch_rcvdata, 170 .disconnect = ng_patch_disconnect, 171 .cmdlist = ng_patch_cmdlist, 172}; 173 174NETGRAPH_INIT(patch, &typestruct); 175 176static int 177ng_patch_constructor(node_p node) 178{ 179 priv_p privdata; 180 181 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO); 182 privdata->dlt = DLT_RAW; 183 184 NG_NODE_SET_PRIVATE(node, privdata); 185 186 return (0); 187} 188 189static int 190ng_patch_newhook(node_p node, hook_p hook, const char *name) 191{ 192 const priv_p privp = NG_NODE_PRIVATE(node); 193 194 if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) { 195 privp->in = hook; 196 } else if (strncmp(name, NG_PATCH_HOOK_OUT, 197 strlen(NG_PATCH_HOOK_OUT)) == 0) { 198 privp->out = hook; 199 } else 200 return (EINVAL); 201 202 return (0); 203} 204 205static int 206ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook) 207{ 208 const priv_p privp = NG_NODE_PRIVATE(node); 209 struct ng_patch_config *conf, *newconf; 210 struct ng_mesg *msg; 211 struct ng_mesg *resp = NULL; 212 int i, error = 0; 213 214 NGI_GET_MSG(item, msg); 215 216 if (msg->header.typecookie != NGM_PATCH_COOKIE) 217 ERROUT(EINVAL); 218 219 switch (msg->header.cmd) 220 { 221 case NGM_PATCH_GETCONFIG: 222 if (privp->conf == NULL) 223 ERROUT(0); 224 225 NG_MKRESPONSE(resp, msg, 226 NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK); 227 228 if (resp == NULL) 229 ERROUT(ENOMEM); 230 231 bcopy(privp->conf, resp->data, 232 NG_PATCH_CONF_SIZE(privp->conf->count)); 233 234 conf = (struct ng_patch_config *) resp->data; 235 236 for (i = 0; i < conf->count; i++) { 237 switch (conf->ops[i].length) 238 { 239 case 1: 240 conf->ops[i].val.v8 = conf->ops[i].val.v1; 241 break; 242 case 2: 243 conf->ops[i].val.v8 = conf->ops[i].val.v2; 244 break; 245 case 4: 246 conf->ops[i].val.v8 = conf->ops[i].val.v4; 247 break; 248 case 8: 249 break; 250 } 251 } 252 253 break; 254 255 case NGM_PATCH_SETCONFIG: 256 conf = (struct ng_patch_config *) msg->data; 257 258 if (msg->header.arglen < sizeof(struct ng_patch_config) || 259 msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count)) 260 ERROUT(EINVAL); 261 262 for (i = 0; i < conf->count; i++) { 263 switch (conf->ops[i].length) 264 { 265 case 1: 266 conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8; 267 break; 268 case 2: 269 conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8; 270 break; 271 case 4: 272 conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8; 273 break; 274 case 8: 275 break; 276 default: 277 ERROUT(EINVAL); 278 } 279 } 280 281 conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6; 282 conf->relative_offset = !!conf->relative_offset; 283 284 newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO); 285 286 bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count)); 287 288 if (privp->conf) 289 free(privp->conf, M_NETGRAPH); 290 291 privp->conf = newconf; 292 293 break; 294 295 case NGM_PATCH_GET_STATS: 296 case NGM_PATCH_CLR_STATS: 297 case NGM_PATCH_GETCLR_STATS: 298 if (msg->header.cmd != NGM_PATCH_CLR_STATS) { 299 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK); 300 301 if (resp == NULL) 302 ERROUT(ENOMEM); 303 304 bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats)); 305 } 306 307 if (msg->header.cmd != NGM_PATCH_GET_STATS) 308 bzero(&(privp->stats), sizeof(struct ng_patch_stats)); 309 310 break; 311 312 case NGM_PATCH_GETDLT: 313 NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK); 314 315 if (resp == NULL) 316 ERROUT(ENOMEM); 317 318 *((uint8_t *) resp->data) = privp->dlt; 319 320 break; 321 322 case NGM_PATCH_SETDLT: 323 if (msg->header.arglen != sizeof(uint8_t)) 324 ERROUT(EINVAL); 325 326 switch (*(uint8_t *) msg->data) 327 { 328 case DLT_EN10MB: 329 case DLT_RAW: 330 privp->dlt = *(uint8_t *) msg->data; 331 break; 332 333 default: 334 ERROUT(EINVAL); 335 } 336 337 break; 338 339 default: 340 ERROUT(EINVAL); 341 } 342 343done: 344 NG_RESPOND_MSG(error, node, item, resp); 345 NG_FREE_MSG(msg); 346 347 return (error); 348} 349 350static void 351do_patch(priv_p privp, struct mbuf *m, int global_offset) 352{ 353 int i, offset, patched = 0; 354 union ng_patch_op_val val; 355 356 for (i = 0; i < privp->conf->count; i++) { 357 offset = global_offset + privp->conf->ops[i].offset; 358 359 if (offset + privp->conf->ops[i].length > m->m_pkthdr.len) 360 continue; 361 362 /* for "=" operation we don't need to copy data from mbuf */ 363 if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET) 364 m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val); 365 366 switch (privp->conf->ops[i].length) 367 { 368 case 1: 369 switch (privp->conf->ops[i].mode) 370 { 371 case NG_PATCH_MODE_SET: 372 val.v1 = privp->conf->ops[i].val.v1; 373 break; 374 case NG_PATCH_MODE_ADD: 375 val.v1 += privp->conf->ops[i].val.v1; 376 break; 377 case NG_PATCH_MODE_SUB: 378 val.v1 -= privp->conf->ops[i].val.v1; 379 break; 380 case NG_PATCH_MODE_MUL: 381 val.v1 *= privp->conf->ops[i].val.v1; 382 break; 383 case NG_PATCH_MODE_DIV: 384 val.v1 /= privp->conf->ops[i].val.v1; 385 break; 386 case NG_PATCH_MODE_NEG: 387 *((int8_t *) &val) = - *((int8_t *) &val); 388 break; 389 case NG_PATCH_MODE_AND: 390 val.v1 &= privp->conf->ops[i].val.v1; 391 break; 392 case NG_PATCH_MODE_OR: 393 val.v1 |= privp->conf->ops[i].val.v1; 394 break; 395 case NG_PATCH_MODE_XOR: 396 val.v1 ^= privp->conf->ops[i].val.v1; 397 break; 398 case NG_PATCH_MODE_SHL: 399 val.v1 <<= privp->conf->ops[i].val.v1; 400 break; 401 case NG_PATCH_MODE_SHR: 402 val.v1 >>= privp->conf->ops[i].val.v1; 403 break; 404 } 405 break; 406 407 case 2: 408 val.v2 = ntohs(val.v2); 409 410 switch (privp->conf->ops[i].mode) 411 { 412 case NG_PATCH_MODE_SET: 413 val.v2 = privp->conf->ops[i].val.v2; 414 break; 415 case NG_PATCH_MODE_ADD: 416 val.v2 += privp->conf->ops[i].val.v2; 417 break; 418 case NG_PATCH_MODE_SUB: 419 val.v2 -= privp->conf->ops[i].val.v2; 420 break; 421 case NG_PATCH_MODE_MUL: 422 val.v2 *= privp->conf->ops[i].val.v2; 423 break; 424 case NG_PATCH_MODE_DIV: 425 val.v2 /= privp->conf->ops[i].val.v2; 426 break; 427 case NG_PATCH_MODE_NEG: 428 *((int16_t *) &val) = - *((int16_t *) &val); 429 break; 430 case NG_PATCH_MODE_AND: 431 val.v2 &= privp->conf->ops[i].val.v2; 432 break; 433 case NG_PATCH_MODE_OR: 434 val.v2 |= privp->conf->ops[i].val.v2; 435 break; 436 case NG_PATCH_MODE_XOR: 437 val.v2 ^= privp->conf->ops[i].val.v2; 438 break; 439 case NG_PATCH_MODE_SHL: 440 val.v2 <<= privp->conf->ops[i].val.v2; 441 break; 442 case NG_PATCH_MODE_SHR: 443 val.v2 >>= privp->conf->ops[i].val.v2; 444 break; 445 } 446 447 val.v2 = htons(val.v2); 448 449 break; 450 451 case 4: 452 val.v4 = ntohl(val.v4); 453 454 switch (privp->conf->ops[i].mode) 455 { 456 case NG_PATCH_MODE_SET: 457 val.v4 = privp->conf->ops[i].val.v4; 458 break; 459 case NG_PATCH_MODE_ADD: 460 val.v4 += privp->conf->ops[i].val.v4; 461 break; 462 case NG_PATCH_MODE_SUB: 463 val.v4 -= privp->conf->ops[i].val.v4; 464 break; 465 case NG_PATCH_MODE_MUL: 466 val.v4 *= privp->conf->ops[i].val.v4; 467 break; 468 case NG_PATCH_MODE_DIV: 469 val.v4 /= privp->conf->ops[i].val.v4; 470 break; 471 case NG_PATCH_MODE_NEG: 472 *((int32_t *) &val) = - *((int32_t *) &val); 473 break; 474 case NG_PATCH_MODE_AND: 475 val.v4 &= privp->conf->ops[i].val.v4; 476 break; 477 case NG_PATCH_MODE_OR: 478 val.v4 |= privp->conf->ops[i].val.v4; 479 break; 480 case NG_PATCH_MODE_XOR: 481 val.v4 ^= privp->conf->ops[i].val.v4; 482 break; 483 case NG_PATCH_MODE_SHL: 484 val.v4 <<= privp->conf->ops[i].val.v4; 485 break; 486 case NG_PATCH_MODE_SHR: 487 val.v4 >>= privp->conf->ops[i].val.v4; 488 break; 489 } 490 491 val.v4 = htonl(val.v4); 492 493 break; 494 495 case 8: 496 val.v8 = be64toh(val.v8); 497 498 switch (privp->conf->ops[i].mode) 499 { 500 case NG_PATCH_MODE_SET: 501 val.v8 = privp->conf->ops[i].val.v8; 502 break; 503 case NG_PATCH_MODE_ADD: 504 val.v8 += privp->conf->ops[i].val.v8; 505 break; 506 case NG_PATCH_MODE_SUB: 507 val.v8 -= privp->conf->ops[i].val.v8; 508 break; 509 case NG_PATCH_MODE_MUL: 510 val.v8 *= privp->conf->ops[i].val.v8; 511 break; 512 case NG_PATCH_MODE_DIV: 513 val.v8 /= privp->conf->ops[i].val.v8; 514 break; 515 case NG_PATCH_MODE_NEG: 516 *((int64_t *) &val) = - *((int64_t *) &val); 517 break; 518 case NG_PATCH_MODE_AND: 519 val.v8 &= privp->conf->ops[i].val.v8; 520 break; 521 case NG_PATCH_MODE_OR: 522 val.v8 |= privp->conf->ops[i].val.v8; 523 break; 524 case NG_PATCH_MODE_XOR: 525 val.v8 ^= privp->conf->ops[i].val.v8; 526 break; 527 case NG_PATCH_MODE_SHL: 528 val.v8 <<= privp->conf->ops[i].val.v8; 529 break; 530 case NG_PATCH_MODE_SHR: 531 val.v8 >>= privp->conf->ops[i].val.v8; 532 break; 533 } 534 535 val.v8 = htobe64(val.v8); 536 537 break; 538 } 539 540 m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val); 541 patched = 1; 542 } 543 544 if (patched) 545 privp->stats.patched++; 546} 547 548static int 549ng_patch_rcvdata(hook_p hook, item_p item) 550{ 551 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 552 struct mbuf *m; 553 hook_p out; 554 int pullup_len = 0; 555 int error = 0; 556 557 priv->stats.received++; 558 559 NGI_GET_M(item, m); 560 561#define PULLUP_CHECK(mbuf, length) do { \ 562 pullup_len += length; \ 563 if (((mbuf)->m_pkthdr.len < pullup_len) || \ 564 (pullup_len > MHLEN)) { \ 565 error = EINVAL; \ 566 goto bypass; \ 567 } \ 568 if ((mbuf)->m_len < pullup_len && \ 569 (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) { \ 570 error = ENOBUFS; \ 571 goto drop; \ 572 } \ 573} while (0) 574 575 if (priv->conf && hook == priv->in && 576 m && (m->m_flags & M_PKTHDR)) { 577 m = m_unshare(m, M_NOWAIT); 578 579 if (m == NULL) 580 ERROUT(ENOMEM); 581 582 if (priv->conf->relative_offset) { 583 struct ether_header *eh; 584 struct ng_patch_vlan_header *vh; 585 uint16_t etype; 586 587 switch (priv->dlt) 588 { 589 case DLT_EN10MB: 590 PULLUP_CHECK(m, sizeof(struct ether_header)); 591 eh = mtod(m, struct ether_header *); 592 etype = ntohs(eh->ether_type); 593 594 for (;;) { /* QinQ support */ 595 switch (etype) 596 { 597 case 0x8100: 598 case 0x88A8: 599 case 0x9100: 600 PULLUP_CHECK(m, sizeof(struct ng_patch_vlan_header)); 601 vh = (struct ng_patch_vlan_header *) mtodo(m, 602 pullup_len - sizeof(struct ng_patch_vlan_header)); 603 etype = ntohs(vh->etype); 604 break; 605 606 default: 607 goto loopend; 608 } 609 } 610loopend: 611 break; 612 613 case DLT_RAW: 614 break; 615 616 default: 617 ERROUT(EINVAL); 618 } 619 } 620 621 do_patch(priv, m, pullup_len); 622 623 m->m_pkthdr.csum_flags |= priv->conf->csum_flags; 624 } 625 626#undef PULLUP_CHECK 627 628bypass: 629 out = NULL; 630 631 if (hook == priv->in) { 632 /* return frames on 'in' hook if 'out' not connected */ 633 out = priv->out ? priv->out : priv->in; 634 } else if (hook == priv->out && priv->in) { 635 /* pass frames on 'out' hook if 'in' connected */ 636 out = priv->in; 637 } 638 639 if (out == NULL) 640 ERROUT(0); 641 642 NG_FWD_NEW_DATA(error, item, out, m); 643 644 return (error); 645 646done: 647drop: 648 NG_FREE_ITEM(item); 649 NG_FREE_M(m); 650 651 priv->stats.dropped++; 652 653 return (error); 654} 655 656static int 657ng_patch_shutdown(node_p node) 658{ 659 const priv_p privdata = NG_NODE_PRIVATE(node); 660 661 NG_NODE_SET_PRIVATE(node, NULL); 662 NG_NODE_UNREF(node); 663 664 if (privdata->conf != NULL) 665 free(privdata->conf, M_NETGRAPH); 666 667 free(privdata, M_NETGRAPH); 668 669 return (0); 670} 671 672static int 673ng_patch_disconnect(hook_p hook) 674{ 675 priv_p priv; 676 677 priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 678 679 if (hook == priv->in) { 680 priv->in = NULL; 681 } 682 683 if (hook == priv->out) { 684 priv->out = NULL; 685 } 686 687 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 && 688 NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */ 689 ng_rmnode_self(NG_HOOK_NODE(hook)); 690 691 return (0); 692} 693