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 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 }