trafgen: l3: Add IPv4 header generation backend

Add L3 module for implement L3 layer protocols generation.

Implemented setting of IPv4 header with all fields except options.
By default IPv4 address of output device is used as src ip address.
On finish (after packet is specified) - total len & checksum are calculated.

Meanwhile Ethernet protocol is initialized as default lower header.

If the lower protocol is IPv4 then IPv4 protocol id is set
to IP-in-IP in lower protocol header.

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
Signed-off-by: Tobias Klauser <tklauser@distanz.ch>

4 files changed, 110 insertions, 0 deletions

diff --git a/trafgen/Makefile b/trafgen/Makefile
index 1c95118..4f342ca 100644
--- a/trafgen/Makefile
+++ b/trafgen/Makefile
@@ -21,6 +21,7 @@ trafgen-objs =	xmalloc.o \
 		cpp.o \
 		trafgen_proto.o \
 		trafgen_l2.o \
+		trafgen_l3.o \
 		trafgen_lexer.yy.o \
 		trafgen_parser.tab.o \
 		trafgen.o
diff --git a/trafgen_l3.c b/trafgen_l3.c
new file mode 100644
index 0000000..dd45395
--- /dev/null
+++ b/trafgen_l3.c
@@ -0,0 +1,82 @@
+/*
+ * netsniff-ng - the packet sniffing beast
+ * Subject to the GPL, version 2.
+ */
+
+#include <linux/if_ether.h>
+
+#include "csum.h"
+#include "built_in.h"
+#include "trafgen_l2.h"
+#include "trafgen_l3.h"
+#include "trafgen_proto.h"
+#include "trafgen_conf.h"
+
+static struct proto_field ipv4_fields[] = {
+	{ .id = IP4_VER,       .len = 1, .offset = 0, .shift = 4, .mask = 0xf0 },
+	{ .id = IP4_IHL,       .len = 1, .offset = 0, .shift = 0, .mask = 0x0f },
+	{ .id = IP4_DSCP,      .len = 1, .offset = 1, .shift = 2, .mask = 0xfc },
+	{ .id = IP4_ECN,       .len = 1, .offset = 1, .shift = 0, .mask = 0x03 },
+	{ .id = IP4_TOS,       .len = 1, .offset = 1 },
+	{ .id = IP4_LEN,       .len = 2, .offset = 2 },
+	{ .id = IP4_ID,        .len = 2, .offset = 4 },
+	{ .id = IP4_FLAGS,     .len = 2, .offset = 6, .shift = 13, .mask = 0xe000 },
+	{ .id = IP4_MF,        .len = 2, .offset = 6, .shift = 13, .mask = 0x2000 },
+	{ .id = IP4_DF,        .len = 2, .offset = 6, .shift = 14, .mask = 0x4000 },
+	{ .id = IP4_FRAG_OFFS, .len = 2, .offset = 6, .shift = 0,  .mask = 0x1fff },
+	{ .id = IP4_TTL,       .len = 1, .offset = 8 },
+	{ .id = IP4_PROTO,     .len = 1, .offset = 9 },
+	{ .id = IP4_CSUM,      .len = 2, .offset = 10 },
+	{ .id = IP4_SADDR,     .len = 4, .offset = 12 },
+	{ .id = IP4_DADDR,     .len = 4, .offset = 16 },
+};
+
+static void ipv4_header_init(struct proto_hdr *hdr)
+{
+	struct proto_hdr *lower;
+
+	proto_lower_default_add(PROTO_ETH);
+
+	lower = proto_current_header();
+
+	if (lower->id == PROTO_ETH)
+		proto_field_set_default_be16(lower, ETH_PROTO_ID, ETH_P_IP);
+	else if (lower->id == PROTO_IP4)
+		proto_field_set_default_u8(lower, IP4_PROTO, IPPROTO_IPIP);
+
+	proto_header_fields_add(hdr, ipv4_fields, array_size(ipv4_fields));
+
+	proto_field_set_default_u8(hdr, IP4_VER, 4);
+	proto_field_set_default_u8(hdr, IP4_IHL, 5);
+	proto_field_set_default_dev_ipv4(hdr, IP4_SADDR);
+}
+
+static void ipv4_packet_finish(struct proto_hdr *hdr)
+{
+	struct packet *pkt = current_packet();
+	uint16_t total_len;
+
+	total_len = pkt->len - hdr->pkt_offset;
+	proto_field_set_default_be16(hdr, IP4_LEN, total_len);
+
+	if (!proto_field_is_set(hdr, IP4_CSUM)) {
+		uint16_t csum;
+		uint8_t ihl;
+
+		ihl = proto_field_get_u8(hdr, IP4_IHL);
+		csum = htons(calc_csum(&pkt->payload[hdr->pkt_offset], ihl * 4));
+		proto_field_set_u16(hdr, IP4_CSUM, bswap_16(csum));
+	}
+}
+
+static struct proto_hdr ipv4_hdr = {
+	.id		= PROTO_IP4,
+	.layer		= PROTO_L3,
+	.header_init	= ipv4_header_init,
+	.packet_finish  = ipv4_packet_finish,
+};
+
+void protos_l3_init(void)
+{
+	proto_header_register(&ipv4_hdr);
+}
diff --git a/trafgen_l3.h b/trafgen_l3.h
new file mode 100644
index 0000000..9427378
--- /dev/null
+++ b/trafgen_l3.h
@@ -0,0 +1,25 @@
+#ifndef TRAFGEN_L3_H
+#define TRAFGEN_L3_H
+
+enum ip4_field {
+	IP4_VER,
+	IP4_IHL,
+	IP4_DSCP,
+	IP4_ECN,
+	IP4_TOS,
+	IP4_LEN,
+	IP4_ID,
+	IP4_FLAGS,
+	IP4_FRAG_OFFS,
+	IP4_TTL,
+	IP4_PROTO,
+	IP4_CSUM,
+	IP4_SADDR,
+	IP4_DADDR,
+	IP4_DF,
+	IP4_MF,
+};
+
+extern void protos_l3_init(void);
+
+#endif /* TRAFGEN_L2_H */
diff --git a/trafgen_proto.c b/trafgen_proto.c
index 2c090b2..75556f9 100644
--- a/trafgen_proto.c
+++ b/trafgen_proto.c
@@ -12,6 +12,7 @@
 #include "xmalloc.h"
 #include "trafgen_conf.h"
 #include "trafgen_l2.h"
+#include "trafgen_l3.h"
 #include "trafgen_proto.h"
 
 #define field_shift_and_mask(f, v) (((v) << (f)->shift) & \
@@ -360,6 +361,7 @@ void protos_init(const char *dev)
 	ctx.dev = dev;
 
 	protos_l2_init();
+	protos_l3_init();
 
 	for (p = registered; p; p = p->next)
 		p->ctx = &ctx;