1 /* 2 * ng_source.c 3 */ 4 5 /*- 6 * Copyright (c) 2005 Gleb Smirnoff <glebius@FreeBSD.org> 7 * Copyright 2002 Sandvine Inc. 8 * All rights reserved. 9 * 10 * Subject to the following obligations and disclaimer of warranty, use and 11 * redistribution of this software, in source or object code forms, with or 12 * without modifications are expressly permitted by Sandvine Inc.; provided, 13 * however, that: 14 * 1. Any and all reproductions of the source or object code must include the 15 * copyright notice above and the following disclaimer of warranties; and 16 * 2. No rights are granted, in any manner or form, to use Sandvine Inc. 17 * trademarks, including the mark "SANDVINE" on advertising, endorsements, 18 * or otherwise except as such appears in the above copyright notice or in 19 * the software. 20 * 21 * THIS SOFTWARE IS BEING PROVIDED BY SANDVINE "AS IS", AND TO THE MAXIMUM 22 * EXTENT PERMITTED BY LAW, SANDVINE MAKES NO REPRESENTATIONS OR WARRANTIES, 23 * EXPRESS OR IMPLIED, REGARDING THIS SOFTWARE, INCLUDING WITHOUT LIMITATION, 24 * ANY AND ALL IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 25 * PURPOSE, OR NON-INFRINGEMENT. SANDVINE DOES NOT WARRANT, GUARANTEE, OR 26 * MAKE ANY REPRESENTATIONS REGARDING THE USE OF, OR THE RESULTS OF THE 27 * USE OF THIS SOFTWARE IN TERMS OF ITS CORRECTNESS, ACCURACY, RELIABILITY 28 * OR OTHERWISE. IN NO EVENT SHALL SANDVINE BE LIABLE FOR ANY DAMAGES 29 * RESULTING FROM OR ARISING OUT OF ANY USE OF THIS SOFTWARE, INCLUDING 30 * WITHOUT LIMITATION, ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 31 * PUNITIVE, OR CONSEQUENTIAL DAMAGES, PROCUREMENT OF SUBSTITUTE GOODS OR 32 * SERVICES, LOSS OF USE, DATA OR PROFITS, HOWEVER CAUSED AND UNDER ANY 33 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 34 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 35 * THIS SOFTWARE, EVEN IF SANDVINE IS ADVISED OF THE POSSIBILITY OF SUCH 36 * DAMAGE. 37 * 38 * Author: Dave Chapeskie 39 */ 40 41 #include <sys/cdefs.h> 42 __FBSDID("$FreeBSD$"); 43 44 /* 45 * This node is used for high speed packet geneneration. It queues 46 * all data recieved on its 'input' hook and when told to start via 47 * a control message it sends the packets out its 'output' hook. In 48 * this way this node can be preloaded with a packet stream which it 49 * can then send continuously as fast as possible. 50 * 51 * Currently it just copies the mbufs as required. It could do various 52 * tricks to try and avoid this. Probably the best performance would 53 * be achieved by modifying the appropriate drivers to be told to 54 * self-re-enqueue packets (e.g. the if_bge driver could reuse the same 55 * transmit descriptors) under control of this node; perhaps via some 56 * flag in the mbuf or some such. The node could peek at an appropriate 57 * ifnet flag to see if such support is available for the connected 58 * interface. 59 */ 60 61 #include <sys/param.h> 62 #include <sys/systm.h> 63 #include <sys/errno.h> 64 #include <sys/kernel.h> 65 #include <sys/malloc.h> 66 #include <sys/mbuf.h> 67 #include <sys/socket.h> 68 #include <sys/syslog.h> 69 #include <net/if.h> 70 #include <net/if_var.h> 71 #include <netgraph/ng_message.h> 72 #include <netgraph/netgraph.h> 73 #include <netgraph/ng_parse.h> 74 #include <netgraph/ng_ether.h> 75 #include <netgraph/ng_source.h> 76 77 #define NG_SOURCE_INTR_TICKS 1 78 #define NG_SOURCE_DRIVER_IFQ_MAXLEN (4*1024) 79 80 #define mtod_off(m,off,t) ((t)(mtod((m),caddr_t)+(off))) 81 82 /* Per node info */ 83 struct privdata { 84 node_p node; 85 hook_p input; 86 hook_p output; 87 struct ng_source_stats stats; 88 struct ifqueue snd_queue; /* packets to send */ 89 struct mbuf *last_packet; /* last pkt in queue */ 90 struct ifnet *output_ifp; 91 struct callout intr_ch; 92 uint64_t packets; /* packets to send */ 93 uint32_t queueOctets; 94 struct ng_source_embed_info embed_timestamp; 95 struct ng_source_embed_cnt_info embed_counter[NG_SOURCE_COUNTERS]; 96 }; 97 typedef struct privdata *sc_p; 98 99 /* Node flags */ 100 #define NG_SOURCE_ACTIVE (NGF_TYPE1) 101 102 /* Netgraph methods */ 103 static ng_constructor_t ng_source_constructor; 104 static ng_rcvmsg_t ng_source_rcvmsg; 105 static ng_shutdown_t ng_source_rmnode; 106 static ng_newhook_t ng_source_newhook; 107 static ng_connect_t ng_source_connect; 108 static ng_rcvdata_t ng_source_rcvdata; 109 static ng_disconnect_t ng_source_disconnect; 110 111 /* Other functions */ 112 static void ng_source_intr(node_p, hook_p, void *, int); 113 static void ng_source_clr_data (sc_p); 114 static int ng_source_start (sc_p, uint64_t); 115 static void ng_source_stop (sc_p); 116 static int ng_source_send (sc_p, int, int *); 117 static int ng_source_store_output_ifp(sc_p, char *); 118 static void ng_source_packet_mod(sc_p, struct mbuf *, 119 int, int, caddr_t, int); 120 static void ng_source_mod_counter(sc_p sc, 121 struct ng_source_embed_cnt_info *cnt, 122 struct mbuf *m, int increment); 123 static int ng_source_dup_mod(sc_p, struct mbuf *, 124 struct mbuf **); 125 126 /* Parse type for timeval */ 127 static const struct ng_parse_struct_field ng_source_timeval_type_fields[] = { 128 { "tv_sec", &ng_parse_int32_type }, 129 { "tv_usec", &ng_parse_int32_type }, 130 { NULL } 131 }; 132 const struct ng_parse_type ng_source_timeval_type = { 133 &ng_parse_struct_type, 134 &ng_source_timeval_type_fields 135 }; 136 137 /* Parse type for struct ng_source_stats */ 138 static const struct ng_parse_struct_field ng_source_stats_type_fields[] 139 = NG_SOURCE_STATS_TYPE_INFO; 140 static const struct ng_parse_type ng_source_stats_type = { 141 &ng_parse_struct_type, 142 &ng_source_stats_type_fields 143 }; 144 145 /* Parse type for struct ng_source_embed_info */ 146 static const struct ng_parse_struct_field ng_source_embed_type_fields[] = 147 NG_SOURCE_EMBED_TYPE_INFO; 148 static const struct ng_parse_type ng_source_embed_type = { 149 &ng_parse_struct_type, 150 &ng_source_embed_type_fields 151 }; 152 153 /* Parse type for struct ng_source_embed_cnt_info */ 154 static const struct ng_parse_struct_field ng_source_embed_cnt_type_fields[] = 155 NG_SOURCE_EMBED_CNT_TYPE_INFO; 156 static const struct ng_parse_type ng_source_embed_cnt_type = { 157 &ng_parse_struct_type, 158 &ng_source_embed_cnt_type_fields 159 }; 160 161 /* List of commands and how to convert arguments to/from ASCII */ 162 static const struct ng_cmdlist ng_source_cmds[] = { 163 { 164 NGM_SOURCE_COOKIE, 165 NGM_SOURCE_GET_STATS, 166 "getstats", 167 NULL, 168 &ng_source_stats_type 169 }, 170 { 171 NGM_SOURCE_COOKIE, 172 NGM_SOURCE_CLR_STATS, 173 "clrstats", 174 NULL, 175 NULL 176 }, 177 { 178 NGM_SOURCE_COOKIE, 179 NGM_SOURCE_GETCLR_STATS, 180 "getclrstats", 181 NULL, 182 &ng_source_stats_type 183 }, 184 { 185 NGM_SOURCE_COOKIE, 186 NGM_SOURCE_START, 187 "start", 188 &ng_parse_uint64_type, 189 NULL 190 }, 191 { 192 NGM_SOURCE_COOKIE, 193 NGM_SOURCE_STOP, 194 "stop", 195 NULL, 196 NULL 197 }, 198 { 199 NGM_SOURCE_COOKIE, 200 NGM_SOURCE_CLR_DATA, 201 "clrdata", 202 NULL, 203 NULL 204 }, 205 { 206 NGM_SOURCE_COOKIE, 207 NGM_SOURCE_SETIFACE, 208 "setiface", 209 &ng_parse_string_type, 210 NULL 211 }, 212 { 213 NGM_SOURCE_COOKIE, 214 NGM_SOURCE_SETPPS, 215 "setpps", 216 &ng_parse_uint32_type, 217 NULL 218 }, 219 { 220 NGM_SOURCE_COOKIE, 221 NGM_SOURCE_SET_TIMESTAMP, 222 "settimestamp", 223 &ng_source_embed_type, 224 NULL 225 }, 226 { 227 NGM_SOURCE_COOKIE, 228 NGM_SOURCE_GET_TIMESTAMP, 229 "gettimestamp", 230 NULL, 231 &ng_source_embed_type 232 }, 233 { 234 NGM_SOURCE_COOKIE, 235 NGM_SOURCE_SET_COUNTER, 236 "setcounter", 237 &ng_source_embed_cnt_type, 238 NULL 239 }, 240 { 241 NGM_SOURCE_COOKIE, 242 NGM_SOURCE_GET_COUNTER, 243 "getcounter", 244 &ng_parse_uint8_type, 245 &ng_source_embed_cnt_type 246 }, 247 { 0 } 248 }; 249 250 /* Netgraph type descriptor */ 251 static struct ng_type ng_source_typestruct = { 252 .version = NG_ABI_VERSION, 253 .name = NG_SOURCE_NODE_TYPE, 254 .constructor = ng_source_constructor, 255 .rcvmsg = ng_source_rcvmsg, 256 .shutdown = ng_source_rmnode, 257 .newhook = ng_source_newhook, 258 .connect = ng_source_connect, 259 .rcvdata = ng_source_rcvdata, 260 .disconnect = ng_source_disconnect, 261 .cmdlist = ng_source_cmds, 262 }; 263 NETGRAPH_INIT(source, &ng_source_typestruct); 264 265 static int ng_source_set_autosrc(sc_p, uint32_t); 266 267 /* 268 * Node constructor 269 */ 270 static int 271 ng_source_constructor(node_p node) 272 { 273 sc_p sc; 274 275 sc = malloc(sizeof(*sc), M_NETGRAPH, M_WAITOK | M_ZERO); 276 277 NG_NODE_SET_PRIVATE(node, sc); 278 sc->node = node; 279 sc->snd_queue.ifq_maxlen = 2048; /* XXX not checked */ 280 ng_callout_init(&sc->intr_ch); 281 282 return (0); 283 } 284 285 /* 286 * Add a hook 287 */ 288 static int 289 ng_source_newhook(node_p node, hook_p hook, const char *name) 290 { 291 sc_p sc = NG_NODE_PRIVATE(node); 292 293 if (strcmp(name, NG_SOURCE_HOOK_INPUT) == 0) { 294 sc->input = hook; 295 } else if (strcmp(name, NG_SOURCE_HOOK_OUTPUT) == 0) { 296 sc->output = hook; 297 sc->output_ifp = 0; 298 bzero(&sc->stats, sizeof(sc->stats)); 299 } else 300 return (EINVAL); 301 302 return (0); 303 } 304 305 /* 306 * Hook has been added 307 */ 308 static int 309 ng_source_connect(hook_p hook) 310 { 311 sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 312 struct ng_mesg *msg; 313 int dummy_error = 0; 314 315 /* 316 * If this is "output" hook, then request information 317 * from our downstream. 318 */ 319 if (hook == sc->output) { 320 NG_MKMESSAGE(msg, NGM_ETHER_COOKIE, NGM_ETHER_GET_IFNAME, 321 0, M_NOWAIT); 322 if (msg == NULL) 323 return (ENOBUFS); 324 325 /* 326 * Our hook and peer hook have HK_INVALID flag set, 327 * so we can't use NG_SEND_MSG_HOOK() macro here. 328 */ 329 NG_SEND_MSG_ID(dummy_error, sc->node, msg, 330 NG_NODE_ID(NG_PEER_NODE(sc->output)), NG_NODE_ID(sc->node)); 331 } 332 333 return (0); 334 } 335 336 /* 337 * Receive a control message 338 */ 339 static int 340 ng_source_rcvmsg(node_p node, item_p item, hook_p lasthook) 341 { 342 sc_p sc = NG_NODE_PRIVATE(node); 343 struct ng_mesg *msg, *resp = NULL; 344 int error = 0; 345 346 NGI_GET_MSG(item, msg); 347 348 switch (msg->header.typecookie) { 349 case NGM_SOURCE_COOKIE: 350 if (msg->header.flags & NGF_RESP) { 351 error = EINVAL; 352 break; 353 } 354 switch (msg->header.cmd) { 355 case NGM_SOURCE_GET_STATS: 356 case NGM_SOURCE_CLR_STATS: 357 case NGM_SOURCE_GETCLR_STATS: 358 { 359 struct ng_source_stats *stats; 360 361 if (msg->header.cmd != NGM_SOURCE_CLR_STATS) { 362 NG_MKRESPONSE(resp, msg, 363 sizeof(*stats), M_NOWAIT); 364 if (resp == NULL) { 365 error = ENOMEM; 366 goto done; 367 } 368 sc->stats.queueOctets = sc->queueOctets; 369 sc->stats.queueFrames = sc->snd_queue.ifq_len; 370 if ((sc->node->nd_flags & NG_SOURCE_ACTIVE) 371 && !timevalisset(&sc->stats.endTime)) { 372 getmicrotime(&sc->stats.elapsedTime); 373 timevalsub(&sc->stats.elapsedTime, 374 &sc->stats.startTime); 375 } 376 stats = (struct ng_source_stats *)resp->data; 377 bcopy(&sc->stats, stats, sizeof(* stats)); 378 } 379 if (msg->header.cmd != NGM_SOURCE_GET_STATS) 380 bzero(&sc->stats, sizeof(sc->stats)); 381 } 382 break; 383 case NGM_SOURCE_START: 384 { 385 uint64_t packets; 386 387 if (msg->header.arglen != sizeof(uint64_t)) { 388 error = EINVAL; 389 break; 390 } 391 392 packets = *(uint64_t *)msg->data; 393 394 error = ng_source_start(sc, packets); 395 396 break; 397 } 398 case NGM_SOURCE_STOP: 399 ng_source_stop(sc); 400 break; 401 case NGM_SOURCE_CLR_DATA: 402 ng_source_clr_data(sc); 403 break; 404 case NGM_SOURCE_SETIFACE: 405 { 406 char *ifname = (char *)msg->data; 407 408 if (msg->header.arglen < 2) { 409 error = EINVAL; 410 break; 411 } 412 413 ng_source_store_output_ifp(sc, ifname); 414 break; 415 } 416 case NGM_SOURCE_SETPPS: 417 { 418 uint32_t pps; 419 420 if (msg->header.arglen != sizeof(uint32_t)) { 421 error = EINVAL; 422 break; 423 } 424 425 pps = *(uint32_t *)msg->data; 426 427 sc->stats.maxPps = pps; 428 429 break; 430 } 431 case NGM_SOURCE_SET_TIMESTAMP: 432 { 433 struct ng_source_embed_info *embed; 434 435 if (msg->header.arglen != sizeof(*embed)) { 436 error = EINVAL; 437 goto done; 438 } 439 embed = (struct ng_source_embed_info *)msg->data; 440 bcopy(embed, &sc->embed_timestamp, sizeof(*embed)); 441 442 break; 443 } 444 case NGM_SOURCE_GET_TIMESTAMP: 445 { 446 struct ng_source_embed_info *embed; 447 448 NG_MKRESPONSE(resp, msg, sizeof(*embed), M_DONTWAIT); 449 if (resp == NULL) { 450 error = ENOMEM; 451 goto done; 452 } 453 embed = (struct ng_source_embed_info *)resp->data; 454 bcopy(&sc->embed_timestamp, embed, sizeof(*embed)); 455 456 break; 457 } 458 case NGM_SOURCE_SET_COUNTER: 459 { 460 struct ng_source_embed_cnt_info *embed; 461 462 if (msg->header.arglen != sizeof(*embed)) { 463 error = EINVAL; 464 goto done; 465 } 466 embed = (struct ng_source_embed_cnt_info *)msg->data; 467 if (embed->index >= NG_SOURCE_COUNTERS || 468 !(embed->width == 1 || embed->width == 2 || 469 embed->width == 4)) { 470 error = EINVAL; 471 goto done; 472 } 473 bcopy(embed, &sc->embed_counter[embed->index], 474 sizeof(*embed)); 475 476 break; 477 } 478 case NGM_SOURCE_GET_COUNTER: 479 { 480 uint8_t index = *(uint8_t *)msg->data; 481 struct ng_source_embed_cnt_info *embed; 482 483 if (index >= NG_SOURCE_COUNTERS) { 484 error = EINVAL; 485 goto done; 486 } 487 NG_MKRESPONSE(resp, msg, sizeof(*embed), M_DONTWAIT); 488 if (resp == NULL) { 489 error = ENOMEM; 490 goto done; 491 } 492 embed = (struct ng_source_embed_cnt_info *)resp->data; 493 bcopy(&sc->embed_counter[index], embed, sizeof(*embed)); 494 495 break; 496 } 497 default: 498 error = EINVAL; 499 break; 500 } 501 break; 502 case NGM_ETHER_COOKIE: 503 if (!(msg->header.flags & NGF_RESP)) { 504 error = EINVAL; 505 break; 506 } 507 switch (msg->header.cmd) { 508 case NGM_ETHER_GET_IFNAME: 509 { 510 char *ifname = (char *)msg->data; 511 512 if (msg->header.arglen < 2) { 513 error = EINVAL; 514 break; 515 } 516 517 if (ng_source_store_output_ifp(sc, ifname) == 0) 518 ng_source_set_autosrc(sc, 0); 519 break; 520 } 521 default: 522 error = EINVAL; 523 } 524 break; 525 default: 526 error = EINVAL; 527 break; 528 } 529 530 done: 531 /* Take care of synchronous response, if any. */ 532 NG_RESPOND_MSG(error, node, item, resp); 533 /* Free the message and return. */ 534 NG_FREE_MSG(msg); 535 return (error); 536 } 537 538 /* 539 * Receive data on a hook 540 * 541 * If data comes in the input hook, enqueue it on the send queue. 542 * If data comes in the output hook, discard it. 543 */ 544 static int 545 ng_source_rcvdata(hook_p hook, item_p item) 546 { 547 sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 548 struct mbuf *m; 549 int error = 0; 550 551 NGI_GET_M(item, m); 552 NG_FREE_ITEM(item); 553 554 /* Which hook? */ 555 if (hook == sc->output) { 556 /* discard */ 557 NG_FREE_M(m); 558 return (error); 559 } 560 KASSERT(hook == sc->input, ("%s: no hook!", __func__)); 561 562 /* Enqueue packet. */ 563 /* XXX should we check IF_QFULL() ? */ 564 _IF_ENQUEUE(&sc->snd_queue, m); 565 sc->queueOctets += m->m_pkthdr.len; 566 sc->last_packet = m; 567 568 return (0); 569 } 570 571 /* 572 * Shutdown processing 573 */ 574 static int 575 ng_source_rmnode(node_p node) 576 { 577 sc_p sc = NG_NODE_PRIVATE(node); 578 579 ng_source_stop(sc); 580 ng_source_clr_data(sc); 581 NG_NODE_SET_PRIVATE(node, NULL); 582 NG_NODE_UNREF(node); 583 free(sc, M_NETGRAPH); 584 585 return (0); 586 } 587 588 /* 589 * Hook disconnection 590 */ 591 static int 592 ng_source_disconnect(hook_p hook) 593 { 594 sc_p sc; 595 596 sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 597 KASSERT(sc != NULL, ("%s: null node private", __func__)); 598 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 || hook == sc->output) 599 ng_rmnode_self(NG_HOOK_NODE(hook)); 600 return (0); 601 } 602 603 /* 604 * Set sc->output_ifp to point to the struct ifnet of the interface 605 * reached via our output hook. 606 */ 607 static int 608 ng_source_store_output_ifp(sc_p sc, char *ifname) 609 { 610 struct ifnet *ifp; 611 int s; 612 613 ifp = ifunit(ifname); 614 615 if (ifp == NULL) { 616 printf("%s: can't find interface %s\n", __func__, ifname); 617 return (EINVAL); 618 } 619 sc->output_ifp = ifp; 620 621 #if 1 622 /* XXX mucking with a drivers ifqueue size is ugly but we need it 623 * to queue a lot of packets to get close to line rate on a gigabit 624 * interface with small packets. 625 * XXX we should restore the original value at stop or disconnect 626 */ 627 s = splimp(); /* XXX is this required? */ 628 if (ifp->if_snd.ifq_maxlen < NG_SOURCE_DRIVER_IFQ_MAXLEN) { 629 printf("ng_source: changing ifq_maxlen from %d to %d\n", 630 ifp->if_snd.ifq_maxlen, NG_SOURCE_DRIVER_IFQ_MAXLEN); 631 ifp->if_snd.ifq_maxlen = NG_SOURCE_DRIVER_IFQ_MAXLEN; 632 } 633 splx(s); 634 #endif 635 return (0); 636 } 637 638 /* 639 * Set the attached ethernet node's ethernet source address override flag. 640 */ 641 static int 642 ng_source_set_autosrc(sc_p sc, uint32_t flag) 643 { 644 struct ng_mesg *msg; 645 int error = 0; 646 647 NG_MKMESSAGE(msg, NGM_ETHER_COOKIE, NGM_ETHER_SET_AUTOSRC, 648 sizeof (uint32_t), M_NOWAIT); 649 if (msg == NULL) 650 return(ENOBUFS); 651 652 *(uint32_t *)msg->data = flag; 653 NG_SEND_MSG_HOOK(error, sc->node, msg, sc->output, 0); 654 return (error); 655 } 656 657 /* 658 * Clear out the data we've queued 659 */ 660 static void 661 ng_source_clr_data (sc_p sc) 662 { 663 struct mbuf *m; 664 665 for (;;) { 666 _IF_DEQUEUE(&sc->snd_queue, m); 667 if (m == NULL) 668 break; 669 NG_FREE_M(m); 670 } 671 sc->queueOctets = 0; 672 sc->last_packet = NULL; 673 } 674 675 /* 676 * Start sending queued data out the output hook 677 */ 678 static int 679 ng_source_start(sc_p sc, uint64_t packets) 680 { 681 if (sc->output_ifp == NULL) { 682 printf("ng_source: start without iface configured\n"); 683 return (ENXIO); 684 } 685 686 if (sc->node->nd_flags & NG_SOURCE_ACTIVE) 687 return (EBUSY); 688 689 sc->node->nd_flags |= NG_SOURCE_ACTIVE; 690 691 sc->packets = packets; 692 timevalclear(&sc->stats.elapsedTime); 693 timevalclear(&sc->stats.endTime); 694 getmicrotime(&sc->stats.startTime); 695 getmicrotime(&sc->stats.lastTime); 696 ng_callout(&sc->intr_ch, sc->node, NULL, 0, 697 ng_source_intr, sc, 0); 698 699 return (0); 700 } 701 702 /* 703 * Stop sending queued data out the output hook 704 */ 705 static void 706 ng_source_stop(sc_p sc) 707 { 708 ng_uncallout(&sc->intr_ch, sc->node); 709 sc->node->nd_flags &= ~NG_SOURCE_ACTIVE; 710 getmicrotime(&sc->stats.endTime); 711 sc->stats.elapsedTime = sc->stats.endTime; 712 timevalsub(&sc->stats.elapsedTime, &sc->stats.startTime); 713 } 714 715 /* 716 * While active called every NG_SOURCE_INTR_TICKS ticks. 717 * Sends as many packets as the interface connected to our 718 * output hook is able to enqueue. 719 */ 720 static void 721 ng_source_intr(node_p node, hook_p hook, void *arg1, int arg2) 722 { 723 sc_p sc = (sc_p)arg1; 724 struct ifqueue *ifq; 725 int packets; 726 727 KASSERT(sc != NULL, ("%s: null node private", __func__)); 728 729 if (sc->packets == 0 || sc->output == NULL 730 || (sc->node->nd_flags & NG_SOURCE_ACTIVE) == 0) { 731 ng_source_stop(sc); 732 return; 733 } 734 735 if (sc->output_ifp != NULL) { 736 ifq = (struct ifqueue *)&sc->output_ifp->if_snd; 737 packets = ifq->ifq_maxlen - ifq->ifq_len; 738 } else 739 packets = sc->snd_queue.ifq_len; 740 741 if (sc->stats.maxPps != 0) { 742 struct timeval now, elapsed; 743 uint64_t usec; 744 int maxpkt; 745 746 getmicrotime(&now); 747 elapsed = now; 748 timevalsub(&elapsed, &sc->stats.lastTime); 749 usec = elapsed.tv_sec * 1000000 + elapsed.tv_usec; 750 maxpkt = (uint64_t)sc->stats.maxPps * usec / 1000000; 751 sc->stats.lastTime = now; 752 if (packets > maxpkt) 753 packets = maxpkt; 754 } 755 756 ng_source_send(sc, packets, NULL); 757 if (sc->packets == 0) 758 ng_source_stop(sc); 759 else 760 ng_callout(&sc->intr_ch, node, NULL, NG_SOURCE_INTR_TICKS, 761 ng_source_intr, sc, 0); 762 } 763 764 /* 765 * Send packets out our output hook. 766 */ 767 static int 768 ng_source_send(sc_p sc, int tosend, int *sent_p) 769 { 770 struct mbuf *m, *m2; 771 int sent; 772 int error = 0; 773 774 KASSERT(tosend >= 0, ("%s: negative tosend param", __func__)); 775 KASSERT(sc->node->nd_flags & NG_SOURCE_ACTIVE, 776 ("%s: inactive node", __func__)); 777 778 if ((uint64_t)tosend > sc->packets) 779 tosend = sc->packets; 780 781 /* Go through the queue sending packets one by one. */ 782 for (sent = 0; error == 0 && sent < tosend; ++sent) { 783 _IF_DEQUEUE(&sc->snd_queue, m); 784 if (m == NULL) 785 break; 786 787 /* Duplicate and modify the packet. */ 788 error = ng_source_dup_mod(sc, m, &m2); 789 if (error) { 790 if (error == ENOBUFS) 791 _IF_PREPEND(&sc->snd_queue, m); 792 else 793 _IF_ENQUEUE(&sc->snd_queue, m); 794 break; 795 } 796 797 /* Re-enqueue the original packet for us. */ 798 _IF_ENQUEUE(&sc->snd_queue, m); 799 800 sc->stats.outFrames++; 801 sc->stats.outOctets += m2->m_pkthdr.len; 802 NG_SEND_DATA_ONLY(error, sc->output, m2); 803 if (error) 804 break; 805 } 806 807 sc->packets -= sent; 808 if (sent_p != NULL) 809 *sent_p = sent; 810 return (error); 811 } 812 813 /* 814 * Modify packet in 'm' by changing 'len' bytes starting at 'offset' 815 * to data in 'cp'. 816 * 817 * The packet data in 'm' must be in a contiguous buffer in a single mbuf. 818 */ 819 static void 820 ng_source_packet_mod(sc_p sc, struct mbuf *m, int offset, int len, caddr_t cp, 821 int flags) 822 { 823 if (len == 0) 824 return; 825 826 /* Can't modify beyond end of packet. */ 827 /* TODO: Pad packet for this case. */ 828 if (offset + len > m->m_len) 829 return; 830 831 bcopy(cp, mtod_off(m, offset, caddr_t), len); 832 } 833 834 static void 835 ng_source_mod_counter(sc_p sc, struct ng_source_embed_cnt_info *cnt, 836 struct mbuf *m, int increment) 837 { 838 caddr_t cp; 839 uint32_t val; 840 841 val = htonl(cnt->next_val); 842 cp = (caddr_t)&val + sizeof(val) - cnt->width; 843 ng_source_packet_mod(sc, m, cnt->offset, cnt->width, cp, cnt->flags); 844 845 if (increment) { 846 cnt->next_val += increment; 847 848 if (increment > 0 && cnt->next_val > cnt->max_val) { 849 cnt->next_val = cnt->min_val - 1 + 850 (cnt->next_val - cnt->max_val); 851 if (cnt->next_val > cnt->max_val) 852 cnt->next_val = cnt->max_val; 853 } else if (increment < 0 && cnt->next_val < cnt->min_val) { 854 cnt->next_val = cnt->max_val + 1 + 855 (cnt->next_val - cnt->min_val); 856 if (cnt->next_val < cnt->min_val) 857 cnt->next_val = cnt->max_val; 858 } 859 } 860 } 861 862 static int 863 ng_source_dup_mod(sc_p sc, struct mbuf *m0, struct mbuf **m_ptr) 864 { 865 struct mbuf *m; 866 struct ng_source_embed_cnt_info *cnt; 867 struct ng_source_embed_info *ts; 868 int modify; 869 int error = 0; 870 int i, increment; 871 872 /* Are we going to modify packets? */ 873 modify = sc->embed_timestamp.flags & NGM_SOURCE_EMBED_ENABLE; 874 for (i = 0; !modify && i < NG_SOURCE_COUNTERS; ++i) 875 modify = sc->embed_counter[i].flags & NGM_SOURCE_EMBED_ENABLE; 876 877 /* Duplicate the packet. */ 878 if (modify) 879 m = m_dup(m0, M_DONTWAIT); 880 else 881 m = m_copypacket(m0, M_DONTWAIT); 882 if (m == NULL) { 883 error = ENOBUFS; 884 goto done; 885 } 886 *m_ptr = m; 887 888 if (!modify) 889 goto done; 890 891 /* Modify the copied packet for sending. */ 892 KASSERT(M_WRITABLE(m), ("%s: packet not writable", __func__)); 893 894 for (i = 0; i < NG_SOURCE_COUNTERS; ++i) { 895 cnt = &sc->embed_counter[i]; 896 if (cnt->flags & NGM_SOURCE_EMBED_ENABLE) { 897 if ((cnt->flags & NGM_SOURCE_INC_CNT_PER_LIST) == 0 || 898 sc->last_packet == m0) 899 increment = cnt->increment; 900 else 901 increment = 0; 902 ng_source_mod_counter(sc, cnt, m, increment); 903 } 904 } 905 906 ts = &sc->embed_timestamp; 907 if (ts->flags & NGM_SOURCE_EMBED_ENABLE) { 908 struct timeval now; 909 getmicrotime(&now); 910 now.tv_sec = htonl(now.tv_sec); 911 now.tv_usec = htonl(now.tv_usec); 912 ng_source_packet_mod(sc, m, ts->offset, sizeof (now), 913 (caddr_t)&now, ts->flags); 914 } 915 916 done: 917 return(error); 918 } 919