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