ng_source.c revision 167156
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 <dchapeskie@sandvine.com> 39 */ 40 41#include <sys/cdefs.h> 42__FBSDID("$FreeBSD: head/sys/netgraph/ng_source.c 167156 2007-03-01 23:16:17Z emaste $"); 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 */ 83struct 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 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}; 95typedef struct privdata *sc_p; 96 97/* Node flags */ 98#define NG_SOURCE_ACTIVE (NGF_TYPE1) 99 100/* Netgraph methods */ 101static ng_constructor_t ng_source_constructor; 102static ng_rcvmsg_t ng_source_rcvmsg; 103static ng_shutdown_t ng_source_rmnode; 104static ng_newhook_t ng_source_newhook; 105static ng_connect_t ng_source_connect; 106static ng_rcvdata_t ng_source_rcvdata; 107static ng_disconnect_t ng_source_disconnect; 108 109/* Other functions */ 110static void ng_source_intr(node_p, hook_p, void *, int); 111static void ng_source_clr_data (sc_p); 112static int ng_source_start (sc_p, uint64_t); 113static void ng_source_stop (sc_p); 114static int ng_source_send (sc_p, int, int *); 115static int ng_source_store_output_ifp(sc_p, char *); 116static void ng_source_packet_mod(sc_p, struct mbuf *, 117 int, int, caddr_t, int); 118static int ng_source_dup_mod(sc_p, struct mbuf *, 119 struct mbuf **); 120 121/* Parse type for timeval */ 122static const struct ng_parse_struct_field ng_source_timeval_type_fields[] = { 123 { "tv_sec", &ng_parse_int32_type }, 124 { "tv_usec", &ng_parse_int32_type }, 125 { NULL } 126}; 127const struct ng_parse_type ng_source_timeval_type = { 128 &ng_parse_struct_type, 129 &ng_source_timeval_type_fields 130}; 131 132/* Parse type for struct ng_source_stats */ 133static const struct ng_parse_struct_field ng_source_stats_type_fields[] 134 = NG_SOURCE_STATS_TYPE_INFO; 135static const struct ng_parse_type ng_source_stats_type = { 136 &ng_parse_struct_type, 137 &ng_source_stats_type_fields 138}; 139 140/* Parse type for struct ng_source_embed_info */ 141static const struct ng_parse_struct_field ng_source_embed_type_fields[] = 142 NG_SOURCE_EMBED_TYPE_INFO; 143static const struct ng_parse_type ng_source_embed_type = { 144 &ng_parse_struct_type, 145 &ng_source_embed_type_fields 146}; 147 148/* List of commands and how to convert arguments to/from ASCII */ 149static const struct ng_cmdlist ng_source_cmds[] = { 150 { 151 NGM_SOURCE_COOKIE, 152 NGM_SOURCE_GET_STATS, 153 "getstats", 154 NULL, 155 &ng_source_stats_type 156 }, 157 { 158 NGM_SOURCE_COOKIE, 159 NGM_SOURCE_CLR_STATS, 160 "clrstats", 161 NULL, 162 NULL 163 }, 164 { 165 NGM_SOURCE_COOKIE, 166 NGM_SOURCE_GETCLR_STATS, 167 "getclrstats", 168 NULL, 169 &ng_source_stats_type 170 }, 171 { 172 NGM_SOURCE_COOKIE, 173 NGM_SOURCE_START, 174 "start", 175 &ng_parse_uint64_type, 176 NULL 177 }, 178 { 179 NGM_SOURCE_COOKIE, 180 NGM_SOURCE_STOP, 181 "stop", 182 NULL, 183 NULL 184 }, 185 { 186 NGM_SOURCE_COOKIE, 187 NGM_SOURCE_CLR_DATA, 188 "clrdata", 189 NULL, 190 NULL 191 }, 192 { 193 NGM_SOURCE_COOKIE, 194 NGM_SOURCE_SETIFACE, 195 "setiface", 196 &ng_parse_string_type, 197 NULL 198 }, 199 { 200 NGM_SOURCE_COOKIE, 201 NGM_SOURCE_SETPPS, 202 "setpps", 203 &ng_parse_uint32_type, 204 NULL 205 }, 206 { 207 NGM_SOURCE_COOKIE, 208 NGM_SOURCE_SET_TIMESTAMP, 209 "settimestamp", 210 &ng_source_embed_type, 211 NULL 212 }, 213 { 214 NGM_SOURCE_COOKIE, 215 NGM_SOURCE_GET_TIMESTAMP, 216 "gettimestamp", 217 NULL, 218 &ng_source_embed_type 219 }, 220 { 0 } 221}; 222 223/* Netgraph type descriptor */ 224static struct ng_type ng_source_typestruct = { 225 .version = NG_ABI_VERSION, 226 .name = NG_SOURCE_NODE_TYPE, 227 .constructor = ng_source_constructor, 228 .rcvmsg = ng_source_rcvmsg, 229 .shutdown = ng_source_rmnode, 230 .newhook = ng_source_newhook, 231 .connect = ng_source_connect, 232 .rcvdata = ng_source_rcvdata, 233 .disconnect = ng_source_disconnect, 234 .cmdlist = ng_source_cmds, 235}; 236NETGRAPH_INIT(source, &ng_source_typestruct); 237 238static int ng_source_set_autosrc(sc_p, uint32_t); 239 240/* 241 * Node constructor 242 */ 243static int 244ng_source_constructor(node_p node) 245{ 246 sc_p sc; 247 248 sc = malloc(sizeof(*sc), M_NETGRAPH, M_NOWAIT | M_ZERO); 249 if (sc == NULL) 250 return (ENOMEM); 251 252 NG_NODE_SET_PRIVATE(node, sc); 253 sc->node = node; 254 sc->snd_queue.ifq_maxlen = 2048; /* XXX not checked */ 255 ng_callout_init(&sc->intr_ch); 256 257 return (0); 258} 259 260/* 261 * Add a hook 262 */ 263static int 264ng_source_newhook(node_p node, hook_p hook, const char *name) 265{ 266 sc_p sc = NG_NODE_PRIVATE(node); 267 268 if (strcmp(name, NG_SOURCE_HOOK_INPUT) == 0) { 269 sc->input = hook; 270 } else if (strcmp(name, NG_SOURCE_HOOK_OUTPUT) == 0) { 271 sc->output = hook; 272 sc->output_ifp = 0; 273 bzero(&sc->stats, sizeof(sc->stats)); 274 } else 275 return (EINVAL); 276 277 return (0); 278} 279 280/* 281 * Hook has been added 282 */ 283static int 284ng_source_connect(hook_p hook) 285{ 286 sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 287 struct ng_mesg *msg; 288 int dummy_error = 0; 289 290 /* 291 * If this is "output" hook, then request information 292 * from our downstream. 293 */ 294 if (hook == sc->output) { 295 NG_MKMESSAGE(msg, NGM_ETHER_COOKIE, NGM_ETHER_GET_IFNAME, 296 0, M_NOWAIT); 297 if (msg == NULL) 298 return (ENOBUFS); 299 300 /* 301 * Our hook and peer hook have HK_INVALID flag set, 302 * so we can't use NG_SEND_MSG_HOOK() macro here. 303 */ 304 NG_SEND_MSG_ID(dummy_error, sc->node, msg, 305 NG_NODE_ID(NG_PEER_NODE(sc->output)), NG_NODE_ID(sc->node)); 306 } 307 308 return (0); 309} 310 311/* 312 * Receive a control message 313 */ 314static int 315ng_source_rcvmsg(node_p node, item_p item, hook_p lasthook) 316{ 317 sc_p sc = NG_NODE_PRIVATE(node); 318 struct ng_mesg *msg, *resp = NULL; 319 int error = 0; 320 321 NGI_GET_MSG(item, msg); 322 323 switch (msg->header.typecookie) { 324 case NGM_SOURCE_COOKIE: 325 if (msg->header.flags & NGF_RESP) { 326 error = EINVAL; 327 break; 328 } 329 switch (msg->header.cmd) { 330 case NGM_SOURCE_GET_STATS: 331 case NGM_SOURCE_CLR_STATS: 332 case NGM_SOURCE_GETCLR_STATS: 333 { 334 struct ng_source_stats *stats; 335 336 if (msg->header.cmd != NGM_SOURCE_CLR_STATS) { 337 NG_MKRESPONSE(resp, msg, 338 sizeof(*stats), M_NOWAIT); 339 if (resp == NULL) { 340 error = ENOMEM; 341 goto done; 342 } 343 sc->stats.queueOctets = sc->queueOctets; 344 sc->stats.queueFrames = sc->snd_queue.ifq_len; 345 if ((sc->node->nd_flags & NG_SOURCE_ACTIVE) 346 && !timevalisset(&sc->stats.endTime)) { 347 getmicrotime(&sc->stats.elapsedTime); 348 timevalsub(&sc->stats.elapsedTime, 349 &sc->stats.startTime); 350 } 351 stats = (struct ng_source_stats *)resp->data; 352 bcopy(&sc->stats, stats, sizeof(* stats)); 353 } 354 if (msg->header.cmd != NGM_SOURCE_GET_STATS) 355 bzero(&sc->stats, sizeof(sc->stats)); 356 } 357 break; 358 case NGM_SOURCE_START: 359 { 360 uint64_t packets; 361 362 if (msg->header.arglen != sizeof(uint64_t)) { 363 error = EINVAL; 364 break; 365 } 366 367 packets = *(uint64_t *)msg->data; 368 369 error = ng_source_start(sc, packets); 370 371 break; 372 } 373 case NGM_SOURCE_STOP: 374 ng_source_stop(sc); 375 break; 376 case NGM_SOURCE_CLR_DATA: 377 ng_source_clr_data(sc); 378 break; 379 case NGM_SOURCE_SETIFACE: 380 { 381 char *ifname = (char *)msg->data; 382 383 if (msg->header.arglen < 2) { 384 error = EINVAL; 385 break; 386 } 387 388 ng_source_store_output_ifp(sc, ifname); 389 break; 390 } 391 case NGM_SOURCE_SETPPS: 392 { 393 uint32_t pps; 394 395 if (msg->header.arglen != sizeof(uint32_t)) { 396 error = EINVAL; 397 break; 398 } 399 400 pps = *(uint32_t *)msg->data; 401 402 sc->stats.maxPps = pps; 403 404 break; 405 } 406 case NGM_SOURCE_SET_TIMESTAMP: 407 { 408 struct ng_source_embed_info *embed; 409 410 embed = (struct ng_source_embed_info *)msg->data; 411 bcopy(embed, &sc->embed_timestamp, sizeof(*embed)); 412 413 break; 414 } 415 case NGM_SOURCE_GET_TIMESTAMP: 416 { 417 struct ng_source_embed_info *embed; 418 419 NG_MKRESPONSE(resp, msg, sizeof(*embed), M_DONTWAIT); 420 if (resp == NULL) { 421 error = ENOMEM; 422 goto done; 423 } 424 embed = (struct ng_source_embed_info *)resp->data; 425 bcopy(&sc->embed_timestamp, embed, sizeof(*embed)); 426 427 break; 428 } 429 default: 430 error = EINVAL; 431 break; 432 } 433 break; 434 case NGM_ETHER_COOKIE: 435 if (!(msg->header.flags & NGF_RESP)) { 436 error = EINVAL; 437 break; 438 } 439 switch (msg->header.cmd) { 440 case NGM_ETHER_GET_IFNAME: 441 { 442 char *ifname = (char *)msg->data; 443 444 if (msg->header.arglen < 2) { 445 error = EINVAL; 446 break; 447 } 448 449 if (ng_source_store_output_ifp(sc, ifname) == 0) 450 ng_source_set_autosrc(sc, 0); 451 break; 452 } 453 default: 454 error = EINVAL; 455 } 456 break; 457 default: 458 error = EINVAL; 459 break; 460 } 461 462done: 463 /* Take care of synchronous response, if any. */ 464 NG_RESPOND_MSG(error, node, item, resp); 465 /* Free the message and return. */ 466 NG_FREE_MSG(msg); 467 return (error); 468} 469 470/* 471 * Receive data on a hook 472 * 473 * If data comes in the input hook, enqueue it on the send queue. 474 * If data comes in the output hook, discard it. 475 */ 476static int 477ng_source_rcvdata(hook_p hook, item_p item) 478{ 479 sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 480 struct mbuf *m; 481 int error = 0; 482 483 NGI_GET_M(item, m); 484 NG_FREE_ITEM(item); 485 486 /* Which hook? */ 487 if (hook == sc->output) { 488 /* discard */ 489 NG_FREE_M(m); 490 return (error); 491 } 492 KASSERT(hook == sc->input, ("%s: no hook!", __func__)); 493 494 /* Enqueue packet. */ 495 /* XXX should we check IF_QFULL() ? */ 496 _IF_ENQUEUE(&sc->snd_queue, m); 497 sc->queueOctets += m->m_pkthdr.len; 498 499 return (0); 500} 501 502/* 503 * Shutdown processing 504 */ 505static int 506ng_source_rmnode(node_p node) 507{ 508 sc_p sc = NG_NODE_PRIVATE(node); 509 510 ng_source_stop(sc); 511 ng_source_clr_data(sc); 512 NG_NODE_SET_PRIVATE(node, NULL); 513 NG_NODE_UNREF(node); 514 free(sc, M_NETGRAPH); 515 516 return (0); 517} 518 519/* 520 * Hook disconnection 521 */ 522static int 523ng_source_disconnect(hook_p hook) 524{ 525 sc_p sc; 526 527 sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 528 KASSERT(sc != NULL, ("%s: null node private", __func__)); 529 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 || hook == sc->output) 530 ng_rmnode_self(NG_HOOK_NODE(hook)); 531 return (0); 532} 533 534/* 535 * Set sc->output_ifp to point to the the struct ifnet of the interface 536 * reached via our output hook. 537 */ 538static int 539ng_source_store_output_ifp(sc_p sc, char *ifname) 540{ 541 struct ifnet *ifp; 542 int s; 543 544 ifp = ifunit(ifname); 545 546 if (ifp == NULL) { 547 printf("%s: can't find interface %d\n", __func__, if_index); 548 return (EINVAL); 549 } 550 sc->output_ifp = ifp; 551 552#if 1 553 /* XXX mucking with a drivers ifqueue size is ugly but we need it 554 * to queue a lot of packets to get close to line rate on a gigabit 555 * interface with small packets. 556 * XXX we should restore the original value at stop or disconnect 557 */ 558 s = splimp(); /* XXX is this required? */ 559 if (ifp->if_snd.ifq_maxlen < NG_SOURCE_DRIVER_IFQ_MAXLEN) { 560 printf("ng_source: changing ifq_maxlen from %d to %d\n", 561 ifp->if_snd.ifq_maxlen, NG_SOURCE_DRIVER_IFQ_MAXLEN); 562 ifp->if_snd.ifq_maxlen = NG_SOURCE_DRIVER_IFQ_MAXLEN; 563 } 564 splx(s); 565#endif 566 return (0); 567} 568 569/* 570 * Set the attached ethernet node's ethernet source address override flag. 571 */ 572static int 573ng_source_set_autosrc(sc_p sc, uint32_t flag) 574{ 575 struct ng_mesg *msg; 576 int error = 0; 577 578 NG_MKMESSAGE(msg, NGM_ETHER_COOKIE, NGM_ETHER_SET_AUTOSRC, 579 sizeof (uint32_t), M_NOWAIT); 580 if (msg == NULL) 581 return(ENOBUFS); 582 583 *(uint32_t *)msg->data = flag; 584 NG_SEND_MSG_HOOK(error, sc->node, msg, sc->output, 0); 585 return (error); 586} 587 588/* 589 * Clear out the data we've queued 590 */ 591static void 592ng_source_clr_data (sc_p sc) 593{ 594 struct mbuf *m; 595 596 for (;;) { 597 _IF_DEQUEUE(&sc->snd_queue, m); 598 if (m == NULL) 599 break; 600 NG_FREE_M(m); 601 } 602 sc->queueOctets = 0; 603} 604 605/* 606 * Start sending queued data out the output hook 607 */ 608static int 609ng_source_start(sc_p sc, uint64_t packets) 610{ 611 if (sc->output_ifp == NULL) { 612 printf("ng_source: start without iface configured\n"); 613 return (ENXIO); 614 } 615 616 if (sc->node->nd_flags & NG_SOURCE_ACTIVE) 617 return (EBUSY); 618 619 sc->node->nd_flags |= NG_SOURCE_ACTIVE; 620 621 sc->packets = packets; 622 timevalclear(&sc->stats.elapsedTime); 623 timevalclear(&sc->stats.endTime); 624 getmicrotime(&sc->stats.startTime); 625 getmicrotime(&sc->stats.lastTime); 626 ng_callout(&sc->intr_ch, sc->node, NULL, 0, 627 ng_source_intr, sc, 0); 628 629 return (0); 630} 631 632/* 633 * Stop sending queued data out the output hook 634 */ 635static void 636ng_source_stop(sc_p sc) 637{ 638 ng_uncallout(&sc->intr_ch, sc->node); 639 sc->node->nd_flags &= ~NG_SOURCE_ACTIVE; 640 getmicrotime(&sc->stats.endTime); 641 sc->stats.elapsedTime = sc->stats.endTime; 642 timevalsub(&sc->stats.elapsedTime, &sc->stats.startTime); 643} 644 645/* 646 * While active called every NG_SOURCE_INTR_TICKS ticks. 647 * Sends as many packets as the interface connected to our 648 * output hook is able to enqueue. 649 */ 650static void 651ng_source_intr(node_p node, hook_p hook, void *arg1, int arg2) 652{ 653 sc_p sc = (sc_p)arg1; 654 struct ifqueue *ifq; 655 int packets; 656 657 KASSERT(sc != NULL, ("%s: null node private", __func__)); 658 659 if (sc->packets == 0 || sc->output == NULL 660 || (sc->node->nd_flags & NG_SOURCE_ACTIVE) == 0) { 661 ng_source_stop(sc); 662 return; 663 } 664 665 if (sc->output_ifp != NULL) { 666 ifq = (struct ifqueue *)&sc->output_ifp->if_snd; 667 packets = ifq->ifq_maxlen - ifq->ifq_len; 668 } else 669 packets = sc->snd_queue.ifq_len; 670 671 if (sc->stats.maxPps != 0) { 672 struct timeval now, elapsed; 673 uint64_t usec; 674 int maxpkt; 675 676 getmicrotime(&now); 677 elapsed = now; 678 timevalsub(&elapsed, &sc->stats.lastTime); 679 usec = elapsed.tv_sec * 1000000 + elapsed.tv_usec; 680 maxpkt = (uint64_t)sc->stats.maxPps * usec / 1000000; 681 sc->stats.lastTime = now; 682 if (packets > maxpkt) 683 packets = maxpkt; 684 } 685 686 ng_source_send(sc, packets, NULL); 687 if (sc->packets == 0) 688 ng_source_stop(sc); 689 else 690 ng_callout(&sc->intr_ch, node, NULL, NG_SOURCE_INTR_TICKS, 691 ng_source_intr, sc, 0); 692} 693 694/* 695 * Send packets out our output hook. 696 */ 697static int 698ng_source_send(sc_p sc, int tosend, int *sent_p) 699{ 700 struct mbuf *m, *m2; 701 int sent; 702 int error = 0; 703 704 KASSERT(tosend >= 0, ("%s: negative tosend param", __func__)); 705 KASSERT(sc->node->nd_flags & NG_SOURCE_ACTIVE, 706 ("%s: inactive node", __func__)); 707 708 if ((uint64_t)tosend > sc->packets) 709 tosend = sc->packets; 710 711 /* Go through the queue sending packets one by one. */ 712 for (sent = 0; error == 0 && sent < tosend; ++sent) { 713 _IF_DEQUEUE(&sc->snd_queue, m); 714 if (m == NULL) 715 break; 716 717 /* Duplicate and modify the packet. */ 718 error = ng_source_dup_mod(sc, m, &m2); 719 if (error) { 720 if (error == ENOBUFS) 721 _IF_PREPEND(&sc->snd_queue, m); 722 else 723 _IF_ENQUEUE(&sc->snd_queue, m); 724 break; 725 } 726 727 /* Re-enqueue the original packet for us. */ 728 _IF_ENQUEUE(&sc->snd_queue, m); 729 730 sc->stats.outFrames++; 731 sc->stats.outOctets += m2->m_pkthdr.len; 732 NG_SEND_DATA_ONLY(error, sc->output, m2); 733 if (error) 734 break; 735 } 736 737 sc->packets -= sent; 738 if (sent_p != NULL) 739 *sent_p = sent; 740 return (error); 741} 742 743/* 744 * Modify packet in 'm' by changing 'len' bytes starting at 'offset' 745 * to data in 'cp'. 746 * 747 * The packet data in 'm' must be in a contiguous buffer in a single mbuf. 748 */ 749static void 750ng_source_packet_mod(sc_p sc, struct mbuf *m, int offset, int len, caddr_t cp, 751 int flags) 752{ 753 if (len == 0) 754 return; 755 756 /* Can't modify beyond end of packet. */ 757 /* TODO: Pad packet for this case. */ 758 if (offset + len > m->m_len) 759 return; 760 761 bcopy(cp, mtod_off(m, offset, caddr_t), len); 762} 763 764static int 765ng_source_dup_mod(sc_p sc, struct mbuf *m0, struct mbuf **m_ptr) 766{ 767 struct mbuf *m; 768 struct ng_source_embed_info *ts; 769 int modify; 770 int error = 0; 771 772 /* Are we going to modify packets? */ 773 modify = sc->embed_timestamp.flags & NGM_SOURCE_EMBED_ENABLE; 774 775 /* Duplicate the packet. */ 776 if (modify) 777 m = m_dup(m0, M_DONTWAIT); 778 else 779 m = m_copypacket(m0, M_DONTWAIT); 780 if (m == NULL) { 781 error = ENOBUFS; 782 goto done; 783 } 784 *m_ptr = m; 785 786 if (!modify) 787 goto done; 788 789 /* Modify the copied packet for sending. */ 790 KASSERT(M_WRITABLE(m), ("%s: packet not writable", __func__)); 791 792 ts = &sc->embed_timestamp; 793 if (ts->flags & NGM_SOURCE_EMBED_ENABLE) { 794 struct timeval now; 795 getmicrotime(&now); 796 now.tv_sec = htonl(now.tv_sec); 797 now.tv_usec = htonl(now.tv_usec); 798 ng_source_packet_mod(sc, m, ts->offset, sizeof (now), 799 (caddr_t)&now, ts->flags); 800 } 801 802done: 803 return(error); 804} 805