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