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