1 /*- 2 * Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com> 3 * Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru> 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25 * SUCH DAMAGE. 26 * 27 */ 28 29 #include <sys/cdefs.h> 30 __FBSDID("$FreeBSD$"); 31 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 67 #define ERROUT(x) { error = (x); goto done; } 68 69 static int 70 ng_patch_config_getlen(const struct ng_parse_type *type, 71 const u_char *start, const u_char *buf) 72 { 73 const struct ng_patch_config *conf; 74 75 conf = (const struct ng_patch_config *)(buf - 76 offsetof(struct ng_patch_config, ops)); 77 78 return (conf->count); 79 } 80 81 static const struct ng_parse_struct_field ng_patch_op_type_fields[] 82 = NG_PATCH_OP_TYPE; 83 static const struct ng_parse_type ng_patch_op_type = { 84 &ng_parse_struct_type, 85 &ng_patch_op_type_fields 86 }; 87 88 static const struct ng_parse_array_info ng_patch_ops_array_info = { 89 &ng_patch_op_type, 90 &ng_patch_config_getlen 91 }; 92 static const struct ng_parse_type ng_patch_ops_array_type = { 93 &ng_parse_array_type, 94 &ng_patch_ops_array_info 95 }; 96 97 static const struct ng_parse_struct_field ng_patch_config_type_fields[] 98 = NG_PATCH_CONFIG_TYPE; 99 static const struct ng_parse_type ng_patch_config_type = { 100 &ng_parse_struct_type, 101 &ng_patch_config_type_fields 102 }; 103 104 static const struct ng_parse_struct_field ng_patch_stats_fields[] 105 = NG_PATCH_STATS_TYPE; 106 static const struct ng_parse_type ng_patch_stats_type = { 107 &ng_parse_struct_type, 108 &ng_patch_stats_fields 109 }; 110 111 static const struct ng_cmdlist ng_patch_cmdlist[] = { 112 { 113 NGM_PATCH_COOKIE, 114 NGM_PATCH_GETDLT, 115 "getdlt", 116 NULL, 117 &ng_parse_uint8_type 118 }, 119 { 120 NGM_PATCH_COOKIE, 121 NGM_PATCH_SETDLT, 122 "setdlt", 123 &ng_parse_uint8_type, 124 NULL 125 }, 126 { 127 NGM_PATCH_COOKIE, 128 NGM_PATCH_GETCONFIG, 129 "getconfig", 130 NULL, 131 &ng_patch_config_type 132 }, 133 { 134 NGM_PATCH_COOKIE, 135 NGM_PATCH_SETCONFIG, 136 "setconfig", 137 &ng_patch_config_type, 138 NULL 139 }, 140 { 141 NGM_PATCH_COOKIE, 142 NGM_PATCH_GET_STATS, 143 "getstats", 144 NULL, 145 &ng_patch_stats_type 146 }, 147 { 148 NGM_PATCH_COOKIE, 149 NGM_PATCH_CLR_STATS, 150 "clrstats", 151 NULL, 152 NULL 153 }, 154 { 155 NGM_PATCH_COOKIE, 156 NGM_PATCH_GETCLR_STATS, 157 "getclrstats", 158 NULL, 159 &ng_patch_stats_type 160 }, 161 { 0 } 162 }; 163 164 static struct ng_type typestruct = { 165 .version = NG_ABI_VERSION, 166 .name = NG_PATCH_NODE_TYPE, 167 .constructor = ng_patch_constructor, 168 .rcvmsg = ng_patch_rcvmsg, 169 .shutdown = ng_patch_shutdown, 170 .newhook = ng_patch_newhook, 171 .rcvdata = ng_patch_rcvdata, 172 .disconnect = ng_patch_disconnect, 173 .cmdlist = ng_patch_cmdlist, 174 }; 175 176 NETGRAPH_INIT(patch, &typestruct); 177 178 static int 179 ng_patch_constructor(node_p node) 180 { 181 priv_p privdata; 182 183 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO); 184 privdata->dlt = DLT_RAW; 185 186 NG_NODE_SET_PRIVATE(node, privdata); 187 188 return (0); 189 } 190 191 static int 192 ng_patch_newhook(node_p node, hook_p hook, const char *name) 193 { 194 const priv_p privp = NG_NODE_PRIVATE(node); 195 196 if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) { 197 privp->in = hook; 198 } else if (strncmp(name, NG_PATCH_HOOK_OUT, 199 strlen(NG_PATCH_HOOK_OUT)) == 0) { 200 privp->out = hook; 201 } else 202 return (EINVAL); 203 204 return (0); 205 } 206 207 static int 208 ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook) 209 { 210 const priv_p privp = NG_NODE_PRIVATE(node); 211 struct ng_patch_config *conf, *newconf; 212 struct ng_mesg *msg; 213 struct ng_mesg *resp = NULL; 214 int i, error = 0; 215 216 NGI_GET_MSG(item, msg); 217 218 if (msg->header.typecookie != NGM_PATCH_COOKIE) 219 ERROUT(EINVAL); 220 221 switch (msg->header.cmd) 222 { 223 case NGM_PATCH_GETCONFIG: 224 if (privp->conf == NULL) 225 ERROUT(0); 226 227 NG_MKRESPONSE(resp, msg, 228 NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK); 229 230 if (resp == NULL) 231 ERROUT(ENOMEM); 232 233 bcopy(privp->conf, resp->data, 234 NG_PATCH_CONF_SIZE(privp->conf->count)); 235 236 conf = (struct ng_patch_config *) resp->data; 237 238 for (i = 0; i < conf->count; i++) { 239 switch (conf->ops[i].length) 240 { 241 case 1: 242 conf->ops[i].val.v8 = conf->ops[i].val.v1; 243 break; 244 case 2: 245 conf->ops[i].val.v8 = conf->ops[i].val.v2; 246 break; 247 case 4: 248 conf->ops[i].val.v8 = conf->ops[i].val.v4; 249 break; 250 case 8: 251 break; 252 } 253 } 254 255 break; 256 257 case NGM_PATCH_SETCONFIG: 258 conf = (struct ng_patch_config *) msg->data; 259 260 if (msg->header.arglen < sizeof(struct ng_patch_config) || 261 msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count)) 262 ERROUT(EINVAL); 263 264 for (i = 0; i < conf->count; i++) { 265 switch (conf->ops[i].length) 266 { 267 case 1: 268 conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8; 269 break; 270 case 2: 271 conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8; 272 break; 273 case 4: 274 conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8; 275 break; 276 case 8: 277 break; 278 default: 279 ERROUT(EINVAL); 280 } 281 } 282 283 conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6; 284 conf->relative_offset = !!conf->relative_offset; 285 286 newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO); 287 288 bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count)); 289 290 if (privp->conf) 291 free(privp->conf, M_NETGRAPH); 292 293 privp->conf = newconf; 294 295 break; 296 297 case NGM_PATCH_GET_STATS: 298 case NGM_PATCH_CLR_STATS: 299 case NGM_PATCH_GETCLR_STATS: 300 if (msg->header.cmd != NGM_PATCH_CLR_STATS) { 301 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK); 302 303 if (resp == NULL) 304 ERROUT(ENOMEM); 305 306 bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats)); 307 } 308 309 if (msg->header.cmd != NGM_PATCH_GET_STATS) 310 bzero(&(privp->stats), sizeof(struct ng_patch_stats)); 311 312 break; 313 314 case NGM_PATCH_GETDLT: 315 NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK); 316 317 if (resp == NULL) 318 ERROUT(ENOMEM); 319 320 *((uint8_t *) resp->data) = privp->dlt; 321 322 break; 323 324 case NGM_PATCH_SETDLT: 325 if (msg->header.arglen != sizeof(uint8_t)) 326 ERROUT(EINVAL); 327 328 switch (*(uint8_t *) msg->data) 329 { 330 case DLT_EN10MB: 331 case DLT_RAW: 332 privp->dlt = *(uint8_t *) msg->data; 333 break; 334 335 default: 336 ERROUT(EINVAL); 337 } 338 339 break; 340 341 default: 342 ERROUT(EINVAL); 343 } 344 345 done: 346 NG_RESPOND_MSG(error, node, item, resp); 347 NG_FREE_MSG(msg); 348 349 return (error); 350 } 351 352 static void 353 do_patch(priv_p privp, struct mbuf *m, int global_offset) 354 { 355 int i, offset, patched = 0; 356 union ng_patch_op_val val; 357 358 for (i = 0; i < privp->conf->count; i++) { 359 offset = global_offset + privp->conf->ops[i].offset; 360 361 if (offset + privp->conf->ops[i].length > m->m_pkthdr.len) 362 continue; 363 364 /* for "=" operation we don't need to copy data from mbuf */ 365 if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET) 366 m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val); 367 368 switch (privp->conf->ops[i].length) 369 { 370 case 1: 371 switch (privp->conf->ops[i].mode) 372 { 373 case NG_PATCH_MODE_SET: 374 val.v1 = privp->conf->ops[i].val.v1; 375 break; 376 case NG_PATCH_MODE_ADD: 377 val.v1 += privp->conf->ops[i].val.v1; 378 break; 379 case NG_PATCH_MODE_SUB: 380 val.v1 -= privp->conf->ops[i].val.v1; 381 break; 382 case NG_PATCH_MODE_MUL: 383 val.v1 *= privp->conf->ops[i].val.v1; 384 break; 385 case NG_PATCH_MODE_DIV: 386 val.v1 /= privp->conf->ops[i].val.v1; 387 break; 388 case NG_PATCH_MODE_NEG: 389 *((int8_t *) &val) = - *((int8_t *) &val); 390 break; 391 case NG_PATCH_MODE_AND: 392 val.v1 &= privp->conf->ops[i].val.v1; 393 break; 394 case NG_PATCH_MODE_OR: 395 val.v1 |= privp->conf->ops[i].val.v1; 396 break; 397 case NG_PATCH_MODE_XOR: 398 val.v1 ^= privp->conf->ops[i].val.v1; 399 break; 400 case NG_PATCH_MODE_SHL: 401 val.v1 <<= privp->conf->ops[i].val.v1; 402 break; 403 case NG_PATCH_MODE_SHR: 404 val.v1 >>= privp->conf->ops[i].val.v1; 405 break; 406 } 407 break; 408 409 case 2: 410 val.v2 = ntohs(val.v2); 411 412 switch (privp->conf->ops[i].mode) 413 { 414 case NG_PATCH_MODE_SET: 415 val.v2 = privp->conf->ops[i].val.v2; 416 break; 417 case NG_PATCH_MODE_ADD: 418 val.v2 += privp->conf->ops[i].val.v2; 419 break; 420 case NG_PATCH_MODE_SUB: 421 val.v2 -= privp->conf->ops[i].val.v2; 422 break; 423 case NG_PATCH_MODE_MUL: 424 val.v2 *= privp->conf->ops[i].val.v2; 425 break; 426 case NG_PATCH_MODE_DIV: 427 val.v2 /= privp->conf->ops[i].val.v2; 428 break; 429 case NG_PATCH_MODE_NEG: 430 *((int16_t *) &val) = - *((int16_t *) &val); 431 break; 432 case NG_PATCH_MODE_AND: 433 val.v2 &= privp->conf->ops[i].val.v2; 434 break; 435 case NG_PATCH_MODE_OR: 436 val.v2 |= privp->conf->ops[i].val.v2; 437 break; 438 case NG_PATCH_MODE_XOR: 439 val.v2 ^= privp->conf->ops[i].val.v2; 440 break; 441 case NG_PATCH_MODE_SHL: 442 val.v2 <<= privp->conf->ops[i].val.v2; 443 break; 444 case NG_PATCH_MODE_SHR: 445 val.v2 >>= privp->conf->ops[i].val.v2; 446 break; 447 } 448 449 val.v2 = htons(val.v2); 450 451 break; 452 453 case 4: 454 val.v4 = ntohl(val.v4); 455 456 switch (privp->conf->ops[i].mode) 457 { 458 case NG_PATCH_MODE_SET: 459 val.v4 = privp->conf->ops[i].val.v4; 460 break; 461 case NG_PATCH_MODE_ADD: 462 val.v4 += privp->conf->ops[i].val.v4; 463 break; 464 case NG_PATCH_MODE_SUB: 465 val.v4 -= privp->conf->ops[i].val.v4; 466 break; 467 case NG_PATCH_MODE_MUL: 468 val.v4 *= privp->conf->ops[i].val.v4; 469 break; 470 case NG_PATCH_MODE_DIV: 471 val.v4 /= privp->conf->ops[i].val.v4; 472 break; 473 case NG_PATCH_MODE_NEG: 474 *((int32_t *) &val) = - *((int32_t *) &val); 475 break; 476 case NG_PATCH_MODE_AND: 477 val.v4 &= privp->conf->ops[i].val.v4; 478 break; 479 case NG_PATCH_MODE_OR: 480 val.v4 |= privp->conf->ops[i].val.v4; 481 break; 482 case NG_PATCH_MODE_XOR: 483 val.v4 ^= privp->conf->ops[i].val.v4; 484 break; 485 case NG_PATCH_MODE_SHL: 486 val.v4 <<= privp->conf->ops[i].val.v4; 487 break; 488 case NG_PATCH_MODE_SHR: 489 val.v4 >>= privp->conf->ops[i].val.v4; 490 break; 491 } 492 493 val.v4 = htonl(val.v4); 494 495 break; 496 497 case 8: 498 val.v8 = be64toh(val.v8); 499 500 switch (privp->conf->ops[i].mode) 501 { 502 case NG_PATCH_MODE_SET: 503 val.v8 = privp->conf->ops[i].val.v8; 504 break; 505 case NG_PATCH_MODE_ADD: 506 val.v8 += privp->conf->ops[i].val.v8; 507 break; 508 case NG_PATCH_MODE_SUB: 509 val.v8 -= privp->conf->ops[i].val.v8; 510 break; 511 case NG_PATCH_MODE_MUL: 512 val.v8 *= privp->conf->ops[i].val.v8; 513 break; 514 case NG_PATCH_MODE_DIV: 515 val.v8 /= privp->conf->ops[i].val.v8; 516 break; 517 case NG_PATCH_MODE_NEG: 518 *((int64_t *) &val) = - *((int64_t *) &val); 519 break; 520 case NG_PATCH_MODE_AND: 521 val.v8 &= privp->conf->ops[i].val.v8; 522 break; 523 case NG_PATCH_MODE_OR: 524 val.v8 |= privp->conf->ops[i].val.v8; 525 break; 526 case NG_PATCH_MODE_XOR: 527 val.v8 ^= privp->conf->ops[i].val.v8; 528 break; 529 case NG_PATCH_MODE_SHL: 530 val.v8 <<= privp->conf->ops[i].val.v8; 531 break; 532 case NG_PATCH_MODE_SHR: 533 val.v8 >>= privp->conf->ops[i].val.v8; 534 break; 535 } 536 537 val.v8 = htobe64(val.v8); 538 539 break; 540 } 541 542 m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val); 543 patched = 1; 544 } 545 546 if (patched) 547 privp->stats.patched++; 548 } 549 550 static int 551 ng_patch_rcvdata(hook_p hook, item_p item) 552 { 553 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 554 struct mbuf *m; 555 hook_p out; 556 int pullup_len = 0; 557 int error = 0; 558 559 priv->stats.received++; 560 561 NGI_GET_M(item, m); 562 563 #define PULLUP_CHECK(mbuf, length) do { \ 564 pullup_len += length; \ 565 if (((mbuf)->m_pkthdr.len < pullup_len) || \ 566 (pullup_len > MHLEN)) { \ 567 error = EINVAL; \ 568 goto bypass; \ 569 } \ 570 if ((mbuf)->m_len < pullup_len && \ 571 (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) { \ 572 error = ENOBUFS; \ 573 goto drop; \ 574 } \ 575 } while (0) 576 577 if (priv->conf && hook == priv->in && 578 m && (m->m_flags & M_PKTHDR)) { 579 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