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