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