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 42 static ng_constructor_t ng_patch_constructor; 43 static ng_rcvmsg_t ng_patch_rcvmsg; 44 static ng_shutdown_t ng_patch_shutdown; 45 static ng_newhook_t ng_patch_newhook; 46 static ng_rcvdata_t ng_patch_rcvdata; 47 static ng_disconnect_t ng_patch_disconnect; 48 49 static int 50 ng_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 60 static const struct ng_parse_struct_field ng_patch_op_type_fields[] 61 = NG_PATCH_OP_TYPE_INFO; 62 static const struct ng_parse_type ng_patch_op_type = { 63 &ng_parse_struct_type, 64 &ng_patch_op_type_fields 65 }; 66 67 static const struct ng_parse_array_info ng_patch_confarr_info = { 68 &ng_patch_op_type, 69 &ng_patch_config_getlen 70 }; 71 static const struct ng_parse_type ng_patch_confarr_type = { 72 &ng_parse_array_type, 73 &ng_patch_confarr_info 74 }; 75 76 static const struct ng_parse_struct_field ng_patch_config_type_fields[] 77 = NG_PATCH_CONFIG_TYPE_INFO; 78 static const struct ng_parse_type ng_patch_config_type = { 79 &ng_parse_struct_type, 80 &ng_patch_config_type_fields 81 }; 82 83 static const struct ng_parse_struct_field ng_patch_stats_fields[] 84 = NG_PATCH_STATS_TYPE_INFO; 85 static const struct ng_parse_type ng_patch_stats_type = { 86 &ng_parse_struct_type, 87 &ng_patch_stats_fields 88 }; 89 90 static 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 129 static 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 }; 140 NETGRAPH_INIT(patch, &typestruct); 141 142 union patch_val { 143 uint8_t v1; 144 uint16_t v2; 145 uint32_t v4; 146 uint64_t v8; 147 }; 148 149 struct 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 }; 156 typedef 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 161 static void do_patch(priv_p conf, struct mbuf *m); 162 163 static int 164 ng_patch_constructor(node_p node) 165 { 166 priv_p privdata; 167 168 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAIT | 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 176 static int 177 ng_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 191 static int 192 ng_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), M_WAIT); 212 bcopy(privp->config, resp->data, 213 NG_PATCH_CONF_SIZE(privp->config->count)); 214 break; 215 case NGM_PATCH_SETCONFIG: 216 { 217 if (msg->header.arglen < 218 sizeof(struct ng_patch_config)) { 219 error = EINVAL; 220 break; 221 } 222 223 conf = (struct ng_patch_config *)msg->data; 224 if (msg->header.arglen < 225 NG_PATCH_CONF_SIZE(conf->count)) { 226 error = EINVAL; 227 break; 228 } 229 230 for(i = 0; i < conf->count; i++) { 231 switch(conf->ops[i].length) { 232 case 1: 233 case 2: 234 case 4: 235 case 8: 236 break; 237 default: 238 error = EINVAL; 239 break; 240 } 241 if (error != 0) 242 break; 243 } 244 245 conf->csum_flags &= CSUM_IP | CSUM_TCP | CSUM_UDP | 246 CSUM_SCTP; 247 248 if (error == 0) { 249 newconf = malloc( 250 NG_PATCH_CONF_SIZE(conf->count), 251 M_NETGRAPH, M_WAIT); 252 newval = malloc(conf->count * 253 sizeof(union patch_val), M_NETGRAPH, 254 M_WAIT); 255 for(i = 0; i < conf->count; i++) { 256 switch (conf->ops[i].length) { 257 case 1: 258 newval[i].v1 = 259 conf->ops[i].value; 260 break; 261 case 2: 262 newval[i].v2 = 263 conf->ops[i].value; 264 break; 265 case 4: 266 newval[i].v4 = 267 conf->ops[i].value; 268 break; 269 case 8: 270 newval[i].v8 = 271 conf->ops[i].value; 272 break; 273 } 274 } 275 bcopy(conf, newconf, 276 NG_PATCH_CONF_SIZE(conf->count)); 277 if (privp->val != NULL) 278 free(privp->val, M_NETGRAPH); 279 privp->val = newval; 280 if (privp->config != NULL) 281 free(privp->config, M_NETGRAPH); 282 privp->config = newconf; 283 } 284 break; 285 } 286 case NGM_PATCH_GETCLR_STATS: 287 clear = 1; 288 /* FALLTHROUGH */ 289 case NGM_PATCH_GET_STATS: 290 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), 291 M_WAIT); 292 bcopy(&(privp->stats), resp->data, 293 sizeof(struct ng_patch_stats)); 294 if (clear == 0) 295 break; 296 /* else FALLTHROUGH */ 297 case NGM_PATCH_CLR_STATS: 298 bzero(&(privp->stats), sizeof(struct ng_patch_stats)); 299 break; 300 default: 301 error = EINVAL; 302 break; 303 } 304 break; 305 default: 306 error = EINVAL; 307 break; 308 } 309 310 NG_RESPOND_MSG(error, node, item, resp); 311 NG_FREE_MSG(msg); 312 return(error); 313 } 314 315 static void 316 do_patch(priv_p privp, struct mbuf *m) 317 { 318 struct ng_patch_config *conf; 319 uint64_t buf; 320 int i, patched; 321 322 conf = privp->config; 323 patched = 0; 324 for(i = 0; i < conf->count; i++) { 325 if (conf->ops[i].offset + conf->ops[i].length > 326 m->m_pkthdr.len) 327 continue; 328 329 /* for "=" operation we don't need to copy data from mbuf */ 330 if (conf->ops[i].mode != NG_PATCH_MODE_SET) { 331 m_copydata(m, conf->ops[i].offset, 332 conf->ops[i].length, (caddr_t)&buf); 333 } 334 335 switch (conf->ops[i].length) { 336 case 1: 337 switch (conf->ops[i].mode) { 338 case NG_PATCH_MODE_SET: 339 *((uint8_t *)&buf) = privp->val[i].v1; 340 break; 341 case NG_PATCH_MODE_ADD: 342 *((uint8_t *)&buf) += privp->val[i].v1; 343 break; 344 case NG_PATCH_MODE_SUB: 345 *((uint8_t *)&buf) -= privp->val[i].v1; 346 break; 347 case NG_PATCH_MODE_MUL: 348 *((uint8_t *)&buf) *= privp->val[i].v1; 349 break; 350 case NG_PATCH_MODE_DIV: 351 *((uint8_t *)&buf) /= privp->val[i].v1; 352 break; 353 case NG_PATCH_MODE_NEG: 354 *((int8_t *)&buf) = - *((int8_t *)&buf); 355 break; 356 case NG_PATCH_MODE_AND: 357 *((uint8_t *)&buf) &= privp->val[i].v1; 358 break; 359 case NG_PATCH_MODE_OR: 360 *((uint8_t *)&buf) |= privp->val[i].v1; 361 break; 362 case NG_PATCH_MODE_XOR: 363 *((uint8_t *)&buf) ^= privp->val[i].v1; 364 break; 365 case NG_PATCH_MODE_SHL: 366 *((uint8_t *)&buf) <<= privp->val[i].v1; 367 break; 368 case NG_PATCH_MODE_SHR: 369 *((uint8_t *)&buf) >>= privp->val[i].v1; 370 break; 371 } 372 break; 373 case 2: 374 *((int16_t *)&buf) = ntohs(*((int16_t *)&buf)); 375 switch (conf->ops[i].mode) { 376 case NG_PATCH_MODE_SET: 377 *((uint16_t *)&buf) = privp->val[i].v2; 378 break; 379 case NG_PATCH_MODE_ADD: 380 *((uint16_t *)&buf) += privp->val[i].v2; 381 break; 382 case NG_PATCH_MODE_SUB: 383 *((uint16_t *)&buf) -= privp->val[i].v2; 384 break; 385 case NG_PATCH_MODE_MUL: 386 *((uint16_t *)&buf) *= privp->val[i].v2; 387 break; 388 case NG_PATCH_MODE_DIV: 389 *((uint16_t *)&buf) /= privp->val[i].v2; 390 break; 391 case NG_PATCH_MODE_NEG: 392 *((int16_t *)&buf) = - *((int16_t *)&buf); 393 break; 394 case NG_PATCH_MODE_AND: 395 *((uint16_t *)&buf) &= privp->val[i].v2; 396 break; 397 case NG_PATCH_MODE_OR: 398 *((uint16_t *)&buf) |= privp->val[i].v2; 399 break; 400 case NG_PATCH_MODE_XOR: 401 *((uint16_t *)&buf) ^= privp->val[i].v2; 402 break; 403 case NG_PATCH_MODE_SHL: 404 *((uint16_t *)&buf) <<= privp->val[i].v2; 405 break; 406 case NG_PATCH_MODE_SHR: 407 *((uint16_t *)&buf) >>= privp->val[i].v2; 408 break; 409 } 410 *((int16_t *)&buf) = htons(*((int16_t *)&buf)); 411 break; 412 case 4: 413 *((int32_t *)&buf) = ntohl(*((int32_t *)&buf)); 414 switch (conf->ops[i].mode) { 415 case NG_PATCH_MODE_SET: 416 *((uint32_t *)&buf) = privp->val[i].v4; 417 break; 418 case NG_PATCH_MODE_ADD: 419 *((uint32_t *)&buf) += privp->val[i].v4; 420 break; 421 case NG_PATCH_MODE_SUB: 422 *((uint32_t *)&buf) -= privp->val[i].v4; 423 break; 424 case NG_PATCH_MODE_MUL: 425 *((uint32_t *)&buf) *= privp->val[i].v4; 426 break; 427 case NG_PATCH_MODE_DIV: 428 *((uint32_t *)&buf) /= privp->val[i].v4; 429 break; 430 case NG_PATCH_MODE_NEG: 431 *((int32_t *)&buf) = - *((int32_t *)&buf); 432 break; 433 case NG_PATCH_MODE_AND: 434 *((uint32_t *)&buf) &= privp->val[i].v4; 435 break; 436 case NG_PATCH_MODE_OR: 437 *((uint32_t *)&buf) |= privp->val[i].v4; 438 break; 439 case NG_PATCH_MODE_XOR: 440 *((uint32_t *)&buf) ^= privp->val[i].v4; 441 break; 442 case NG_PATCH_MODE_SHL: 443 *((uint32_t *)&buf) <<= privp->val[i].v4; 444 break; 445 case NG_PATCH_MODE_SHR: 446 *((uint32_t *)&buf) >>= privp->val[i].v4; 447 break; 448 } 449 *((int32_t *)&buf) = htonl(*((int32_t *)&buf)); 450 break; 451 case 8: 452 *((int64_t *)&buf) = be64toh(*((int64_t *)&buf)); 453 switch (conf->ops[i].mode) { 454 case NG_PATCH_MODE_SET: 455 *((uint64_t *)&buf) = privp->val[i].v8; 456 break; 457 case NG_PATCH_MODE_ADD: 458 *((uint64_t *)&buf) += privp->val[i].v8; 459 break; 460 case NG_PATCH_MODE_SUB: 461 *((uint64_t *)&buf) -= privp->val[i].v8; 462 break; 463 case NG_PATCH_MODE_MUL: 464 *((uint64_t *)&buf) *= privp->val[i].v8; 465 break; 466 case NG_PATCH_MODE_DIV: 467 *((uint64_t *)&buf) /= privp->val[i].v8; 468 break; 469 case NG_PATCH_MODE_NEG: 470 *((int64_t *)&buf) = - *((int64_t *)&buf); 471 break; 472 case NG_PATCH_MODE_AND: 473 *((uint64_t *)&buf) &= privp->val[i].v8; 474 break; 475 case NG_PATCH_MODE_OR: 476 *((uint64_t *)&buf) |= privp->val[i].v8; 477 break; 478 case NG_PATCH_MODE_XOR: 479 *((uint64_t *)&buf) ^= privp->val[i].v8; 480 break; 481 case NG_PATCH_MODE_SHL: 482 *((uint64_t *)&buf) <<= privp->val[i].v8; 483 break; 484 case NG_PATCH_MODE_SHR: 485 *((uint64_t *)&buf) >>= privp->val[i].v8; 486 break; 487 } 488 *((int64_t *)&buf) = htobe64(*((int64_t *)&buf)); 489 break; 490 } 491 492 m_copyback(m, conf->ops[i].offset, conf->ops[i].length, 493 (caddr_t)&buf); 494 patched = 1; 495 } 496 if (patched > 0) 497 privp->stats.patched++; 498 } 499 500 static int 501 ng_patch_rcvdata(hook_p hook, item_p item) 502 { 503 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 504 struct mbuf *m; 505 hook_p target; 506 int error; 507 508 priv->stats.received++; 509 NGI_GET_M(item, m); 510 if (priv->config != NULL && hook == priv->in && 511 (m->m_flags & M_PKTHDR) != 0) { 512 m = m_unshare(m,M_NOWAIT); 513 if (m == NULL) { 514 priv->stats.dropped++; 515 NG_FREE_ITEM(item); 516 return (ENOMEM); 517 } 518 do_patch(priv, m); 519 m->m_flags |= priv->config->csum_flags; 520 } 521 522 target = NULL; 523 if (hook == priv->in) { 524 /* return frames on 'in' hook if 'out' not connected */ 525 if (priv->out != NULL) 526 target = priv->out; 527 else 528 target = priv->in; 529 } 530 if (hook == priv->out && priv->in != NULL) 531 target = priv->in; 532 533 if (target == NULL) { 534 priv->stats.dropped++; 535 NG_FREE_ITEM(item); 536 NG_FREE_M(m); 537 return (0); 538 } 539 NG_FWD_NEW_DATA(error, item, target, m); 540 return (error); 541 } 542 543 static int 544 ng_patch_shutdown(node_p node) 545 { 546 const priv_p privdata = NG_NODE_PRIVATE(node); 547 548 if (privdata->val != NULL) 549 free(privdata->val, M_NETGRAPH); 550 if (privdata->config != NULL) 551 free(privdata->config, M_NETGRAPH); 552 NG_NODE_SET_PRIVATE(node, NULL); 553 NG_NODE_UNREF(node); 554 free(privdata, M_NETGRAPH); 555 return (0); 556 } 557 558 static int 559 ng_patch_disconnect(hook_p hook) 560 { 561 priv_p priv; 562 563 priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 564 if (hook == priv->in) { 565 priv->in = NULL; 566 } 567 if (hook == priv->out) { 568 priv->out = NULL; 569 } 570 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 && 571 NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */ 572 ng_rmnode_self(NG_HOOK_NODE(hook)); 573 return (0); 574 } 575 576