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