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