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