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