sys/netgraph/ng_source.c

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