Add "settimestamp" and "gettimestamp" messages, providing the the ability

to embed a timestamp (struct timeval) in outgoing packets.  The message
specifies the offset at which the timestamp should be inserted.

NG_SOURCE(4) gives an example usage that queues an ICMP packet.  Using that
example, the following command will insert a timestamp in the ICMP's data
payload:

  ngctl msg src0: settimestamp '{ offset=0x2a flags=1 }'

Sponsored by:	Sandvine Incorporated

2 files changed, 139 insertions, 5 deletions

diff --git a/sys/netgraph/ng_source.c b/sys/netgraph/ng_source.c
index 26cb43b..df983eb 100644
--- a/sys/netgraph/ng_source.c
+++ b/sys/netgraph/ng_source.c
@@ -77,6 +77,8 @@ __FBSDID("$FreeBSD$");
 #define NG_SOURCE_INTR_TICKS		1
 #define NG_SOURCE_DRIVER_IFQ_MAXLEN	(4*1024)
 
+#define	mtod_off(m,off,t)	((t)(mtod((m),caddr_t)+(off)))
+
 /* Per node info */
 struct privdata {
 	node_p				node;
@@ -88,6 +90,7 @@ struct privdata {
 	struct callout			intr_ch;
 	uint64_t			packets;	/* packets to send */
 	uint32_t			queueOctets;
+	struct ng_source_embed_info	embed_timestamp;
 };
 typedef struct privdata *sc_p;
 
@@ -110,6 +113,10 @@ static int		ng_source_start (sc_p, uint64_t);
 static void		ng_source_stop (sc_p);
 static int		ng_source_send (sc_p, int, int *);
 static int		ng_source_store_output_ifp(sc_p, char *);
+static void		ng_source_packet_mod(sc_p, struct mbuf *,
+			    int, int, caddr_t, int);
+static int		ng_source_dup_mod(sc_p, struct mbuf *,
+			    struct mbuf **);
 
 /* Parse type for timeval */
 static const struct ng_parse_struct_field ng_source_timeval_type_fields[] = {
@@ -130,6 +137,14 @@ static const struct ng_parse_type ng_source_stats_type = {
 	&ng_source_stats_type_fields
 };
 
+/* Parse type for struct ng_source_embed_info */
+static const struct ng_parse_struct_field ng_source_embed_type_fields[] =
+	NG_SOURCE_EMBED_TYPE_INFO;
+static const struct ng_parse_type ng_source_embed_type = {
+	&ng_parse_struct_type,
+	&ng_source_embed_type_fields
+};
+
 /* List of commands and how to convert arguments to/from ASCII */
 static const struct ng_cmdlist ng_source_cmds[] = {
 	{
@@ -188,6 +203,20 @@ static const struct ng_cmdlist ng_source_cmds[] = {
 	  &ng_parse_uint32_type,
 	  NULL
 	},
+	{
+	  NGM_SOURCE_COOKIE,
+	  NGM_SOURCE_SET_TIMESTAMP,
+	  "settimestamp",
+	  &ng_source_embed_type,
+	  NULL
+	},
+	{
+	  NGM_SOURCE_COOKIE,
+	  NGM_SOURCE_GET_TIMESTAMP,
+	  "gettimestamp",
+	  NULL,
+	  &ng_source_embed_type
+	},
 	{ 0 }
 };
 
@@ -374,6 +403,29 @@ ng_source_rcvmsg(node_p node, item_p item, hook_p lasthook)
 
 			break;
 		    }
+		case NGM_SOURCE_SET_TIMESTAMP:
+		    {
+			struct ng_source_embed_info *embed;
+
+			embed = (struct ng_source_embed_info *)msg->data;
+			bcopy(embed, &sc->embed_timestamp, sizeof(*embed));
+
+			break;
+		    }
+		case NGM_SOURCE_GET_TIMESTAMP:
+		    {
+			struct ng_source_embed_info *embed;
+
+			NG_MKRESPONSE(resp, msg, sizeof(*embed), M_DONTWAIT);
+			if (resp == NULL) {
+				error = ENOMEM;
+				goto done;
+			}
+			embed = (struct ng_source_embed_info *)resp->data;
+			bcopy(&sc->embed_timestamp, embed, sizeof(*embed));
+
+			break;
+		    }
 		default:
 			error = EINVAL;
 			break;
@@ -662,11 +714,13 @@ ng_source_send(sc_p sc, int tosend, int *sent_p)
 		if (m == NULL)
 			break;
 
-		/* Duplicate the packet. */
-		m2 = m_copypacket(m, M_DONTWAIT);
-		if (m2 == NULL) {
-			_IF_PREPEND(&sc->snd_queue, m);
-			error = ENOBUFS;
+		/* Duplicate and modify the packet. */
+		error = ng_source_dup_mod(sc, m, &m2);
+		if (error) {
+			if (error == ENOBUFS)
+				_IF_PREPEND(&sc->snd_queue, m);
+			else
+				_IF_ENQUEUE(&sc->snd_queue, m);
 			break;
 		}
 
@@ -685,3 +739,66 @@ ng_source_send(sc_p sc, int tosend, int *sent_p)
 		*sent_p = sent;
 	return (error);
 }
+
+/*
+ * Modify packet in 'm' by changing 'len' bytes starting at 'offset'
+ * to data in 'cp'.
+ *
+ * The packet data in 'm' must be in a contiguous buffer in a single mbuf.
+ */
+static void
+ng_source_packet_mod(sc_p sc, struct mbuf *m, int offset, int len, caddr_t cp,
+    int flags)
+{
+	if (len == 0)
+		return;
+
+	/* Can't modify beyond end of packet. */
+	/* TODO: Pad packet for this case. */
+	if (offset + len > m->m_len)
+		return;
+
+	bcopy(cp, mtod_off(m, offset, caddr_t), len);
+}
+
+static int
+ng_source_dup_mod(sc_p sc, struct mbuf *m0, struct mbuf **m_ptr)
+{
+	struct mbuf *m;
+	struct ng_source_embed_info *ts;
+	int modify;
+	int error = 0;
+
+	/* Are we going to modify packets? */
+	modify = sc->embed_timestamp.flags & NGM_SOURCE_EMBED_ENABLE;
+
+	/* Duplicate the packet. */
+	if (modify)
+		m = m_dup(m0, M_DONTWAIT);
+	else
+		m = m_copypacket(m0, M_DONTWAIT);
+	if (m == NULL) {
+		error = ENOBUFS;
+		goto done;
+	}
+	*m_ptr = m;
+
+	if (!modify)
+		goto done;
+
+	/* Modify the copied packet for sending. */
+	KASSERT(M_WRITABLE(m), ("%s: packet not writable", __func__));
+
+	ts = &sc->embed_timestamp;
+	if (ts->flags & NGM_SOURCE_EMBED_ENABLE) {
+		struct timeval now;
+		getmicrotime(&now);
+		now.tv_sec = htonl(now.tv_sec);
+		now.tv_usec = htonl(now.tv_usec);
+		ng_source_packet_mod(sc, m, ts->offset, sizeof (now),
+		    (caddr_t)&now, ts->flags);
+	}
+
+done:
+	return(error);
+}
diff --git a/sys/netgraph/ng_source.h b/sys/netgraph/ng_source.h
index fcfb151..b3db2cf 100644
--- a/sys/netgraph/ng_source.h
+++ b/sys/netgraph/ng_source.h
@@ -78,6 +78,21 @@ extern const struct ng_parse_type ng_source_timeval_type;
 	  { NULL }						\
 }
 
+/* Packet embedding info for NGM_SOURCE_GET/SET_TIMESTAMP */
+struct ng_source_embed_info {
+	uint16_t	offset;		/* offset from ethernet header */
+	uint8_t		flags;
+	uint8_t		spare;
+};
+#define NGM_SOURCE_EMBED_ENABLE		0x01	/* enable embedding */
+
+/* Keep this in sync with the above structure definition. */
+#define NG_SOURCE_EMBED_TYPE_INFO {				\
+	{ "offset",		&ng_parse_hint16_type	},	\
+	{ "flags",		&ng_parse_hint8_type	},	\
+	{ NULL }						\
+}
+
 /* Netgraph commands */
 enum {
 	NGM_SOURCE_GET_STATS = 1,	/* get stats */
@@ -88,6 +103,8 @@ enum {
 	NGM_SOURCE_CLR_DATA,		/* clear the queued data */
 	NGM_SOURCE_SETIFACE,		/* configure downstream iface */
 	NGM_SOURCE_SETPPS,		/* rate-limiting packets per second */
+	NGM_SOURCE_SET_TIMESTAMP,	/* embed xmit timestamp */
+	NGM_SOURCE_GET_TIMESTAMP,
 };
 
 #endif /* _NETGRAPH_NG_SOURCE_H_ */