sys/netgraph/ng_source.c

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