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
ng_source_constructor(node_p node)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
ng_source_newhook(node_p node,hook_p hook,const char * name)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
ng_source_connect(hook_p hook)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
ng_source_rcvmsg(node_p node,item_p item,hook_p lasthook)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
ng_source_rcvdata(hook_p hook,item_p item)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
ng_source_rmnode(node_p node)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
ng_source_disconnect(hook_p hook)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
ng_source_store_output_ifp(sc_p sc,char * ifname)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
ng_source_set_autosrc(sc_p sc,uint32_t flag)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
ng_source_clr_data(sc_p sc)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
ng_source_start(sc_p sc,uint64_t packets)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
ng_source_stop(sc_p sc)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
ng_source_intr(node_p node,hook_p hook,void * arg1,int arg2)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
ng_source_send(sc_p sc,int tosend,int * sent_p)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
ng_source_packet_mod(sc_p sc,struct mbuf * m,int offset,int len,caddr_t cp,int flags)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
ng_source_mod_counter(sc_p sc,struct ng_source_embed_cnt_info * cnt,struct mbuf * m,int increment)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
ng_source_dup_mod(sc_p sc,struct mbuf * m0,struct mbuf ** m_ptr)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 }
932