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>

1 files changed, 82 insertions, 0 deletions

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);
+}