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