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