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 */
82struct 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};
96typedef struct privdata *sc_p;
97
98/* Node flags */
99#define NG_SOURCE_ACTIVE	(NGF_TYPE1)
100
101/* Netgraph methods */
102static ng_constructor_t	ng_source_constructor;
103static ng_rcvmsg_t	ng_source_rcvmsg;
104static ng_shutdown_t	ng_source_rmnode;
105static ng_newhook_t	ng_source_newhook;
106static ng_connect_t	ng_source_connect;
107static ng_rcvdata_t	ng_source_rcvdata;
108static ng_disconnect_t	ng_source_disconnect;
109
110/* Other functions */
111static void		ng_source_intr(node_p, hook_p, void *, int);
112static void		ng_source_clr_data (sc_p);
113static int		ng_source_start (sc_p, uint64_t);
114static void		ng_source_stop (sc_p);
115static int		ng_source_send (sc_p, int, int *);
116static int		ng_source_store_output_ifp(sc_p, char *);
117static void		ng_source_packet_mod(sc_p, struct mbuf *,
118			    int, int, caddr_t, int);
119static void		ng_source_mod_counter(sc_p sc,
120			    struct ng_source_embed_cnt_info *cnt,
121			    struct mbuf *m, int increment);
122static int		ng_source_dup_mod(sc_p, struct mbuf *,
123			    struct mbuf **);
124
125/* Parse type for timeval */
126static 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};
139const 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 */
145static const struct ng_parse_struct_field ng_source_stats_type_fields[]
146	= NG_SOURCE_STATS_TYPE_INFO;
147static 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 */
153static const struct ng_parse_struct_field ng_source_embed_type_fields[] =
154	NG_SOURCE_EMBED_TYPE_INFO;
155static 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 */
161static const struct ng_parse_struct_field ng_source_embed_cnt_type_fields[] =
162	NG_SOURCE_EMBED_CNT_TYPE_INFO;
163static 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 */
169static 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 */
258static 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};
270NETGRAPH_INIT(source, &ng_source_typestruct);
271
272static int ng_source_set_autosrc(sc_p, uint32_t);
273
274/*
275 * Node constructor
276 */
277static int
278ng_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 */
295static int
296ng_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 */
315static int
316ng_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 */
346static int
347ng_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
537done:
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 */
551static int
552ng_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 */
584static int
585ng_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 */
601static int
602ng_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 */
617static int
618ng_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 */
648static int
649ng_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 */
667static void
668ng_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 */
685static int
686ng_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 */
712static void
713ng_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 */
727static void
728ng_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 */
774static int
775ng_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 */
832static void
833ng_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
847static void
848ng_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
875static int
876ng_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
929done:
930	return(error);
931}
932