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