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