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