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