From 439af62bca4794d78d53fb4634f560d6a75f0adb Mon Sep 17 00:00:00 2001
From: Vadim Kochan <vadim4j@gmail.com>
Date: Sat, 29 Jul 2017 12:46:09 +0300
Subject: trafgen: Dump proto headers in *.cfg format

Added trafgen_dump.c module which dumps headers from packet
in .cfg format. Packet is dumped if -o <file>.cfg was specified,
it might be useful to specify *.pcap file as input and convert it
into .cfg file to edit proto fields in human readable format.

To make it possible several main changes were added:

    1) packet id is embedded into struct packet.id, and
       it is updated on each realloc_packet()

    2) Added new struct proto_hdr.get_next_proto callback
       to make possible apply fields of next header.

    3) Added new dev_io ops for writting packets into .cfg file,
       to re-use common dev_io mechsnism for packets dumping.

Before dump the default ETH_PROTO fields are applied as first header and
then next proto_hdr is identified via .get_next_proto(...) callback.

Meanwhile only eth, arp, vlan, ip4, udp, & tcp protos can be dissected
into *.cfg format.

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
Signed-off-by: Tobias Klauser <tklauser@distanz.ch>
---
 trafgen.8        |   4 +-
 trafgen.c        |  54 ++++++------
 trafgen/Makefile |   1 +
 trafgen_conf.h   |   2 +
 trafgen_dev.c    |  36 +++++++-
 trafgen_dev.h    |   4 +-
 trafgen_dump.c   | 258 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 trafgen_dump.h   |   8 ++
 trafgen_l2.c     |  33 +++++++
 trafgen_l3.c     |  21 ++++-
 trafgen_parser.y |   5 ++
 trafgen_proto.c  |  69 +++++++++++++--
 trafgen_proto.h  |   7 ++
 13 files changed, 460 insertions(+), 42 deletions(-)
 create mode 100644 trafgen_dump.c
 create mode 100644 trafgen_dump.h

diff --git a/trafgen.8 b/trafgen.8
index 67aaaf9..f720043 100644
--- a/trafgen.8
+++ b/trafgen.8
@@ -74,9 +74,9 @@ It is also possible to specify PCAP file with .pcap extension via -i,--in option
 by default packets will be sent at rate considering timestamp from PCAP file which
 might be reset via -b/-t options.
 .PP
-.SS -o <dev|pcap>, -d <dev|pcap>, --out <dev|pcap>, --dev <dev|pcap>
+.SS -o <dev|.pcap|.cfg>, -d <dev|.pcap|.cfg>, --out <dev|.pcap|.cfg>, --dev <dev|.pcap|.cfg>
 Defines the outgoing networking device such as eth0, wlan0 and others or
-a pcap file.
+a *.pcap or *.cfg file. Pcap and configuration files are identified by extension.
 .PP
 .SS -p, --cpp
 Pass the packet configuration to the C preprocessor before reading it into
diff --git a/trafgen.c b/trafgen.c
index 97ac046..9b54399 100644
--- a/trafgen.c
+++ b/trafgen.c
@@ -185,31 +185,31 @@ static void __noreturn help(void)
 	puts("http://www.netsniff-ng.org\n\n"
 	     "Usage: trafgen [options] [packet]\n"
 	     "Options:\n"
-	     "  -i|-c|--in|--conf <cfg/->      Packet configuration file/stdin\n"
-	     "  -o|-d|--out|--dev <netdev>     Networking device i.e., eth0\n"
-	     "  -p|--cpp                       Run packet config through C preprocessor\n"
-	     "  -D|--define                    Add macro/define for C preprocessor\n"
-	     "  -J|--jumbo-support             Support 64KB super jumbo frames (def: 2048B)\n"
-	     "  -R|--rfraw                     Inject raw 802.11 frames\n"
-	     "  -s|--smoke-test <ipv4>         Probe if machine survived fuzz-tested packet\n"
-	     "  -n|--num <uint>                Number of packets until exit (def: 0)\n"
-	     "  -r|--rand                      Randomize packet selection (def: round robin)\n"
-	     "  -P|--cpus <uint>               Specify number of forks(<= CPUs) (def: #CPUs)\n"
-	     "  -t|--gap <time>                Set approx. interpacket gap (s/ms/us/ns, def: us)\n"
-	     "  -b|--rate <rate>               Send traffic at specified rate (pps/B/kB/MB/GB/kbit/Mbit/Gbit/KiB/MiB/GiB)\n"
-	     "  -S|--ring-size <size>          Manually set mmap size (KiB/MiB/GiB)\n"
-	     "  -E|--seed <uint>               Manually set srand(3) seed\n"
-	     "  -u|--user <userid>             Drop privileges and change to userid\n"
-	     "  -g|--group <groupid>           Drop privileges and change to groupid\n"
-	     "  -H|--prio-high                 Make this high priority process\n"
-	     "  -A|--no-sock-mem               Don't tune core socket memory\n"
-	     "  -Q|--notouch-irq               Do not touch IRQ CPU affinity of NIC\n"
-	     "  -q|--qdisc-path                Enable qdisc kernel path (default off since 3.14)\n"
-	     "  -V|--verbose                   Be more verbose\n"
-	     "  -C|--no-cpu-stats              Do not print CPU time statistics on exit\n"
-	     "  -v|--version                   Show version and exit\n"
-	     "  -e|--example                   Show built-in packet config example\n"
-	     "  -h|--help                      Guess what?!\n\n"
+	     "  -i|-c|--in|--conf <cfg/->             Packet configuration file/stdin\n"
+	     "  -o|-d|--out|--dev <netdev|.cfg|.pcap> Networking device or configuration file i.e., eth0\n"
+	     "  -p|--cpp                              Run packet config through C preprocessor\n"
+	     "  -D|--define                           Add macro/define for C preprocessor\n"
+	     "  -J|--jumbo-support                    Support 64KB super jumbo frames (def: 2048B)\n"
+	     "  -R|--rfraw                            Inject raw 802.11 frames\n"
+	     "  -s|--smoke-test <ipv4>                Probe if machine survived fuzz-tested packet\n"
+	     "  -n|--num <uint>                       Number of packets until exit (def: 0)\n"
+	     "  -r|--rand                             Randomize packet selection (def: round robin)\n"
+	     "  -P|--cpus <uint>                      Specify number of forks(<= CPUs) (def: #CPUs)\n"
+	     "  -t|--gap <time>                       Set approx. interpacket gap (s/ms/us/ns, def: us)\n"
+	     "  -b|--rate <rate>                      Send traffic at specified rate (pps/B/kB/MB/GB/kbit/Mbit/Gbit/KiB/MiB/GiB)\n"
+	     "  -S|--ring-size <size>                 Manually set mmap size (KiB/MiB/GiB)\n"
+	     "  -E|--seed <uint>                      Manually set srand(3) seed\n"
+	     "  -u|--user <userid>                    Drop privileges and change to userid\n"
+	     "  -g|--group <groupid>                  Drop privileges and change to groupid\n"
+	     "  -H|--prio-high                        Make this high priority process\n"
+	     "  -A|--no-sock-mem                      Don't tune core socket memory\n"
+	     "  -Q|--notouch-irq                      Do not touch IRQ CPU affinity of NIC\n"
+	     "  -q|--qdisc-path                       Enable qdisc kernel path (default off since 3.14)\n"
+	     "  -V|--verbose                          Be more verbose\n"
+	     "  -C|--no-cpu-stats                     Do not print CPU time statistics on exit\n"
+	     "  -v|--version                          Show version and exit\n"
+	     "  -e|--example                          Show built-in packet config example\n"
+	     "  -h|--help                             Guess what?!\n\n"
 	     "Examples:\n"
 	     "  trafgen --dev eth0 --conf trafgen.cfg\n"
 	     "  trafgen -e | trafgen -i - -o eth0 --cpp -n 1\n"
@@ -1284,8 +1284,8 @@ int main(int argc, char **argv)
 
 	protos_init(ctx.dev_out);
 
-	if (shaper_is_set(&ctx.sh) || (ctx.dev_in && dev_io_is_pcap(ctx.dev_in))
-	    || dev_io_is_pcap(ctx.dev_out)) {
+	if (shaper_is_set(&ctx.sh) || (ctx.dev_in && !dev_io_is_netdev(ctx.dev_in))
+	    || !dev_io_is_netdev(ctx.dev_out)) {
 
 		prctl(PR_SET_TIMERSLACK, 1UL);
 		/* Fall back to single core to not mess up correct timing.
diff --git a/trafgen/Makefile b/trafgen/Makefile
index 381f94d..a1bff80 100644
--- a/trafgen/Makefile
+++ b/trafgen/Makefile
@@ -26,6 +26,7 @@ trafgen-objs =	xmalloc.o \
 		pcap_mm.o \
 		iosched.o \
 		trafgen_dev.o \
+		trafgen_dump.o \
 		trafgen_proto.o \
 		trafgen_l2.o \
 		trafgen_l3.o \
diff --git a/trafgen_conf.h b/trafgen_conf.h
index 7e922fe..a0bbe43 100644
--- a/trafgen_conf.h
+++ b/trafgen_conf.h
@@ -37,11 +37,13 @@ struct csum16 {
 };
 
 struct packet {
+	uint32_t id;
 	uint8_t *payload;
 	size_t len;
 	struct proto_hdr *headers[PROTO_MAX_LAYERS];
 	size_t headers_count;
 	struct timespec tstamp;
+	bool is_created;
 };
 
 struct packet_dyn {
diff --git a/trafgen_dev.c b/trafgen_dev.c
index d613cce..f65442f 100644
--- a/trafgen_dev.c
+++ b/trafgen_dev.c
@@ -20,6 +20,7 @@
 #include "linktype.h"
 #include "trafgen_dev.h"
 #include "trafgen_conf.h"
+#include "trafgen_dump.h"
 
 static int dev_pcap_open(struct dev_io *dev, const char *name, enum dev_io_mode_t mode)
 {
@@ -90,6 +91,7 @@ static struct packet *dev_pcap_read(struct dev_io *dev)
 	pkt = realloc_packet();
 
 	pkt->len = pkt_len;
+	pkt->is_created = true;
 	pkt->payload = xzmalloc(pkt_len);
 	memcpy(pkt->payload, buf, pkt_len);
 	pcap_get_tstamp(&phdr, dev->pcap_magic, &pkt->tstamp);
@@ -97,7 +99,7 @@ static struct packet *dev_pcap_read(struct dev_io *dev)
 	return pkt;
 }
 
-static int dev_pcap_write(struct dev_io *dev, const struct packet *pkt)
+static int dev_pcap_write(struct dev_io *dev, struct packet *pkt)
 {
 	uint8_t *buf = pkt->payload;
 	size_t len = pkt->len;
@@ -172,7 +174,7 @@ static int dev_net_open(struct dev_io *dev, const char *name, enum dev_io_mode_t
 	return 0;
 }
 
-static int dev_net_write(struct dev_io *dev, const struct packet *pkt)
+static int dev_net_write(struct dev_io *dev, struct packet *pkt)
 {
 	struct sockaddr_ll saddr = {
 		.sll_family = PF_PACKET,
@@ -215,6 +217,31 @@ static const struct dev_io_ops dev_net_ops = {
 	.close = dev_net_close,
 };
 
+static int dev_cfg_open(struct dev_io *dev, const char *name, enum dev_io_mode_t mode)
+{
+	dev->fd = open_or_die_m(name, O_RDWR | O_CREAT | O_TRUNC | O_LARGEFILE, DEFFILEMODE);
+	return 0;
+}
+
+static int dev_cfg_write(struct dev_io *dev, struct packet *pkt)
+{
+	if (packet_dump_fd(pkt, dev->fd))
+		return -1;
+
+	return pkt->len;
+}
+
+static void dev_cfg_close(struct dev_io *dev)
+{
+	close(dev->fd);
+}
+
+static const struct dev_io_ops dev_cfg_ops = {
+	.open = dev_cfg_open,
+	.write = dev_cfg_write,
+	.close = dev_cfg_close,
+};
+
 struct dev_io *dev_io_open(const char *name, enum dev_io_mode_t mode)
 {
 	struct dev_io *dev = xzmalloc(sizeof(struct dev_io));
@@ -222,6 +249,9 @@ struct dev_io *dev_io_open(const char *name, enum dev_io_mode_t mode)
 	if (strstr(name, ".pcap")) {
 		dev->name = xstrdup(name);
 		dev->ops = &dev_pcap_ops;
+	} else if (strstr(name, ".cfg")) {
+		dev->name = xstrdup(name);
+		dev->ops = &dev_cfg_ops;
 	} else if (device_mtu(name) > 0) {
 		dev->name = xstrndup(name, IFNAMSIZ);
 		dev->ops = &dev_net_ops;
@@ -240,7 +270,7 @@ struct dev_io *dev_io_open(const char *name, enum dev_io_mode_t mode)
 	return dev;
 };
 
-int dev_io_write(struct dev_io *dev, const struct packet *pkt)
+int dev_io_write(struct dev_io *dev, struct packet *pkt)
 {
 	bug_on(!dev);
 	bug_on(!dev->ops);
diff --git a/trafgen_dev.h b/trafgen_dev.h
index 6708eb8..80086d7 100644
--- a/trafgen_dev.h
+++ b/trafgen_dev.h
@@ -33,14 +33,14 @@ struct dev_io {
 
 struct dev_io_ops {
 	int(*open) (struct dev_io *dev, const char *name, enum dev_io_mode_t mode);
-	int(*write) (struct dev_io *dev, const struct packet *pkt);
+	int(*write) (struct dev_io *dev, struct packet *pkt);
 	struct packet *(*read) (struct dev_io *dev);
 	int(*set_link_type) (struct dev_io *dev, int link_type);
 	void(*close) (struct dev_io *dev);
 };
 
 extern struct dev_io *dev_io_open(const char *name, enum dev_io_mode_t mode);
-extern int dev_io_write(struct dev_io *dev, const struct packet *pkt);
+extern int dev_io_write(struct dev_io *dev, struct packet *pkt);
 extern struct packet *dev_io_read(struct dev_io *dev);
 extern int dev_io_ifindex_get(struct dev_io *dev);
 extern int dev_io_fd_get(struct dev_io *dev);
diff --git a/trafgen_dump.c b/trafgen_dump.c
new file mode 100644
index 0000000..ff68e8b
--- /dev/null
+++ b/trafgen_dump.c
@@ -0,0 +1,258 @@
+/*
+ * netsniff-ng - the packet sniffing beast
+ * Subject to the GPL, version 2.
+ */
+
+#include <stdio.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <net/ethernet.h>
+
+#include "trafgen_l2.h"
+#include "trafgen_l3.h"
+#include "trafgen_l4.h"
+#include "trafgen_proto.h"
+#include "trafgen_dump.h"
+
+#define DUMP(fmt, ...) fprintf((ctx)->file, fmt, ##__VA_ARGS__)
+
+#define PKT_START() DUMP("{\n")
+#define PKT_END() DUMP("}\n")
+
+#define HDR_START(h) DUMP("  %s(", h)
+#define HDR_END(h) DUMP("  ),\n")
+
+#define FIELD_START(fmt, ...) DUMP(fmt",\n", ##__VA_ARGS__)
+#define FIELD_END(fmt, ...) DUMP("    "fmt"\n", ##__VA_ARGS__)
+#define FIELD(fmt, ...) DUMP("    "fmt",\n", ##__VA_ARGS__)
+
+struct dump_ctx {
+	FILE *file;
+};
+
+static int proto_dump_eth(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	uint8_t *mac;
+
+	HDR_START("eth");
+
+	mac = proto_hdr_field_get_bytes(hdr, ETH_DST_ADDR);
+	FIELD_START("da=%02x:%02x:%02x:%02x:%02x:%02x",
+		mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+
+	mac = proto_hdr_field_get_bytes(hdr, ETH_SRC_ADDR);
+	FIELD("sa=%02x:%02x:%02x:%02x:%02x:%02x",
+		mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+
+	FIELD_END("type=0x%x", proto_hdr_field_get_u16(hdr, ETH_TYPE));
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_vlan(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	HDR_START("vlan");
+
+	FIELD_START("id=%d", proto_hdr_field_get_u16(hdr, VLAN_VID));
+	FIELD("pcp=%d", proto_hdr_field_get_u16(hdr, VLAN_PCP));
+	FIELD("dei=%d", proto_hdr_field_get_u16(hdr, VLAN_DEI));
+	FIELD_END("tpid=0x%x", proto_hdr_field_get_u16(hdr, VLAN_TPID));
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_arp(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	char ip_str[INET_ADDRSTRLEN];
+	uint16_t oper;
+	uint8_t *mac;
+	uint32_t ip;
+
+	HDR_START("arp");
+
+	mac = proto_hdr_field_get_bytes(hdr, ARP_SHA);
+	FIELD_START("smac=%02x:%02x:%02x:%02x:%02x:%02x",
+		mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+
+	ip = proto_hdr_field_get_be32(hdr, ARP_SPA);
+	inet_ntop(AF_INET, &ip, ip_str, sizeof(ip_str));
+	FIELD("sip=%s", ip_str);
+
+	mac = proto_hdr_field_get_bytes(hdr, ARP_THA);
+	FIELD("tmac=%02x:%02x:%02x:%02x:%02x:%02x",
+		mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+
+	ip = proto_hdr_field_get_be32(hdr, ARP_TPA);
+	inet_ntop(AF_INET, &ip, ip_str, sizeof(ip_str));
+	FIELD("tip=%s", ip_str);
+
+	oper = proto_hdr_field_get_u16(hdr, ARP_OPER);
+
+	if (oper == ARPOP_REQUEST)
+		FIELD_END("op=request");
+	else if (oper == ARPOP_REPLY)
+		FIELD_END("op=reply");
+	else
+		FIELD_END("op=0x%x", oper);
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_ip4(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	char ip_sa_str[INET_ADDRSTRLEN];
+	char ip_da_str[INET_ADDRSTRLEN];
+	uint32_t ip;
+
+	ip = proto_hdr_field_get_be32(hdr, IP4_SADDR);
+	inet_ntop(AF_INET, &ip, ip_sa_str, sizeof(ip_sa_str));
+
+	ip = proto_hdr_field_get_be32(hdr, IP4_DADDR);
+	inet_ntop(AF_INET, &ip, ip_da_str, sizeof(ip_da_str));
+
+	HDR_START("ip4");
+
+	FIELD_START("ver=0x%x", proto_hdr_field_get_u8(hdr, IP4_VER));
+	FIELD("ihl=0x%x", proto_hdr_field_get_u8(hdr, IP4_IHL));
+	FIELD("dscp=0x%x", proto_hdr_field_get_u8(hdr, IP4_DSCP));
+	FIELD("ecn=0x%x", proto_hdr_field_get_u8(hdr, IP4_ECN));
+	FIELD("tos=0x%x", proto_hdr_field_get_u8(hdr, IP4_TOS));
+	FIELD("len=%d", proto_hdr_field_get_u16(hdr, IP4_LEN));
+	FIELD("id=0x%x", proto_hdr_field_get_u16(hdr, IP4_ID));
+	FIELD("flags=0x%x", proto_hdr_field_get_u16(hdr, IP4_FLAGS));
+	if (proto_hdr_field_get_u16(hdr, IP4_MF))
+		FIELD("mf");
+	if (proto_hdr_field_get_u16(hdr, IP4_DF))
+		FIELD("df");
+	FIELD("frag=0x%x", proto_hdr_field_get_u16(hdr, IP4_FRAG_OFFS));
+	FIELD("ttl=%d", proto_hdr_field_get_u8(hdr, IP4_TTL));
+	FIELD("proto=0x%x", proto_hdr_field_get_u8(hdr, IP4_PROTO));
+	FIELD("csum=0x%x", proto_hdr_field_get_u16(hdr, IP4_CSUM));
+	FIELD("sa=%s", ip_sa_str);
+	FIELD_END("da=%s", ip_da_str);
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_udp(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	HDR_START("udp");
+
+	FIELD_START("dp=%d", proto_hdr_field_get_u16(hdr, UDP_SPORT));
+	FIELD("sp=%d", proto_hdr_field_get_u16(hdr, UDP_DPORT));
+	FIELD("len=%d", proto_hdr_field_get_u16(hdr, UDP_LEN));
+	FIELD_END("csum=0x%x", proto_hdr_field_get_u16(hdr, UDP_CSUM));
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_tcp(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	HDR_START("tcp");
+
+	FIELD_START("dp=%d", proto_hdr_field_get_u16(hdr, TCP_SPORT));
+	FIELD("sp=%d", proto_hdr_field_get_u16(hdr, TCP_DPORT));
+	FIELD("seq=0x%x", proto_hdr_field_get_u32(hdr, TCP_SEQ));
+	FIELD("ackseq=0x%x", proto_hdr_field_get_u32(hdr, TCP_ACK_SEQ));
+	FIELD("hlen=%d", proto_hdr_field_get_u16(hdr, TCP_DOFF));
+	if (proto_hdr_field_get_u16(hdr, TCP_CWR))
+		FIELD("cwr");
+	if (proto_hdr_field_get_u16(hdr, TCP_ECE))
+		FIELD("ecn");
+	if (proto_hdr_field_get_u16(hdr, TCP_URG))
+		FIELD("urg");
+	if (proto_hdr_field_get_u16(hdr, TCP_ACK))
+		FIELD("ack");
+	if (proto_hdr_field_get_u16(hdr, TCP_PSH))
+		FIELD("psh");
+	if (proto_hdr_field_get_u16(hdr, TCP_RST))
+		FIELD("rst");
+	if (proto_hdr_field_get_u16(hdr, TCP_SYN))
+		FIELD("syn");
+	if (proto_hdr_field_get_u16(hdr, TCP_FIN))
+		FIELD("fin");
+	FIELD("win=%d", proto_hdr_field_get_u16(hdr, TCP_WINDOW));
+	FIELD("csum=0x%x", proto_hdr_field_get_u16(hdr, TCP_CSUM));
+	FIELD_END("urgptr=0x%x", proto_hdr_field_get_u16(hdr, TCP_URG_PTR));
+
+	HDR_END();
+	return 0;
+}
+
+static int proto_dump_hdr(struct dump_ctx *ctx, struct proto_hdr *hdr)
+{
+	switch (hdr->ops->id) {
+	case PROTO_ETH:
+		return proto_dump_eth(ctx, hdr);
+	case PROTO_VLAN:
+		return proto_dump_vlan(ctx, hdr);
+	case PROTO_ARP:
+		return proto_dump_arp(ctx, hdr);
+	case PROTO_IP4:
+		return proto_dump_ip4(ctx, hdr);
+	case PROTO_UDP:
+		return proto_dump_udp(ctx, hdr);
+	case PROTO_TCP:
+		return proto_dump_tcp(ctx, hdr);
+	default:
+		return -1;
+	}
+}
+
+int packet_dump_fd(struct packet *pkt, int fd)
+{
+	struct proto_hdr *hdr;
+	enum proto_id pid;
+	struct dump_ctx _ctx;
+	struct dump_ctx *ctx = &_ctx;
+	size_t dump_len = 0;
+	uint32_t i;
+
+	_ctx.file = fdopen(fd, "w");
+	if (!_ctx.file)
+		return -1;
+
+	/* handle case if there is already proto headers */
+	if (pkt->headers_count == 0) {
+		hdr = proto_packet_apply(PROTO_ETH, pkt);
+
+		while ((pid = proto_hdr_get_next_proto(hdr)) != __PROTO_MAX) {
+			if (hdr->pkt_offset + hdr->len >= pkt->len)
+				break;
+
+			hdr = proto_packet_apply(pid, pkt);
+		}
+	}
+
+	PKT_START();
+	for (i = 0; i < pkt->headers_count; i++) {
+		hdr = pkt->headers[i];
+
+		if (proto_dump_hdr(ctx, hdr))
+			break;
+
+		dump_len += hdr->len;
+	}
+
+	/* print rest as a bytes */
+	if (dump_len < pkt->len) {
+		int j = 1;
+
+		DUMP("  ");
+		for (i = dump_len; i < pkt->len; ++i, ++j) {
+			if (j % 15 == 0)
+				DUMP("\n  ");
+			DUMP("0x%02x,", pkt->payload[i]);
+		}
+		DUMP("\n");
+	}
+	PKT_END();
+
+	fflush(ctx->file);
+	return 0;
+}
diff --git a/trafgen_dump.h b/trafgen_dump.h
new file mode 100644
index 0000000..860a956
--- /dev/null
+++ b/trafgen_dump.h
@@ -0,0 +1,8 @@
+#ifndef TRAFGEN_DUMP_H
+#define TRAFGEN_DUMP_H
+
+#include "trafgen_conf.h"
+
+extern int packet_dump_fd(struct packet *pkt, int fd);
+
+#endif /* TRAFGEN_DUMP_H */
diff --git a/trafgen_l2.c b/trafgen_l2.c
index 4858c5f..48c6f6f 100644
--- a/trafgen_l2.c
+++ b/trafgen_l2.c
@@ -40,11 +40,37 @@ static uint16_t pid_to_eth(enum proto_id pid)
 	}
 }
 
+static uint16_t eth_to_pid(uint16_t etype)
+{
+	switch (etype) {
+	case ETH_P_ARP:
+		return PROTO_ARP;
+	case ETH_P_IP:
+		return PROTO_IP4;
+	case ETH_P_IPV6:
+		return PROTO_IP6;
+	case ETH_P_MPLS_UC:
+		return PROTO_MPLS;
+	case ETH_P_8021Q:
+	case ETH_P_8021AD:
+		return PROTO_VLAN;
+	case ETH_P_PAUSE:
+		return PROTO_PAUSE;
+	default:
+		return __PROTO_MAX;
+	}
+}
+
 static void eth_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
 {
 	proto_hdr_field_set_default_be16(hdr, ETH_TYPE, pid_to_eth(pid));
 }
 
+static enum proto_id eth_get_next_proto(struct proto_hdr *hdr)
+{
+	return eth_to_pid(proto_hdr_field_get_u16(hdr, ETH_TYPE));
+}
+
 static void eth_header_init(struct proto_hdr *hdr)
 {
 	proto_header_fields_add(hdr, eth_fields, array_size(eth_fields));
@@ -59,6 +85,7 @@ static const struct proto_ops eth_proto_ops = {
 	.layer		= PROTO_L2,
 	.header_init	= eth_header_init,
 	.set_next_proto = eth_set_next_proto,
+	.get_next_proto = eth_get_next_proto,
 };
 
 static struct proto_field pause_fields[] = {
@@ -158,11 +185,17 @@ static void vlan_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
 		proto_hdr_field_set_be16(hdr, VLAN_ETYPE, pid_to_eth(pid));
 }
 
+static enum proto_id vlan_get_next_proto(struct proto_hdr *hdr)
+{
+	return eth_to_pid(proto_hdr_field_get_u16(hdr, VLAN_ETYPE));
+}
+
 static const struct proto_ops vlan_proto_ops = {
 	.id		= PROTO_VLAN,
 	.layer		= PROTO_L2,
 	.header_init	= vlan_header_init,
 	.set_next_proto = vlan_set_next_proto,
+	.get_next_proto = vlan_get_next_proto,
 };
 
 static struct proto_field arp_fields[] = {
diff --git a/trafgen_l3.c b/trafgen_l3.c
index 48790e5..a3f711d 100644
--- a/trafgen_l3.c
+++ b/trafgen_l3.c
@@ -38,7 +38,7 @@ static void ipv4_header_init(struct proto_hdr *hdr)
 	struct dev_io *dev = proto_dev_get();
 
 	/* In case of tun interface we do not need to create Ethernet header */
-	if (dev_io_is_pcap(dev) || device_type(dev_io_name_get(dev)) != ARPHRD_NONE)
+	if (!dev_io_is_netdev(dev) || device_type(dev_io_name_get(dev)) != ARPHRD_NONE)
 		proto_lower_default_add(hdr, PROTO_ETH);
 
 	proto_header_fields_add(hdr, ipv4_fields, array_size(ipv4_fields));
@@ -117,6 +117,24 @@ static void ipv4_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
 	proto_hdr_field_set_default_u8(hdr, IP4_PROTO, ip_proto);
 }
 
+static enum proto_id ipv4_get_next_proto(struct proto_hdr *hdr)
+{
+	switch (proto_hdr_field_get_u8(hdr, IP4_PROTO)) {
+	case IPPROTO_IPIP:
+		return PROTO_IP4;
+	case IPPROTO_IPV6:
+		return PROTO_IP6;
+	case IPPROTO_ICMP:
+		return PROTO_ICMP4;
+	case IPPROTO_UDP:
+		return PROTO_UDP;
+	case IPPROTO_TCP:
+		return PROTO_TCP;
+	default:
+		return __PROTO_MAX;
+	}
+}
+
 static const struct proto_ops ipv4_proto_ops = {
 	.id		= PROTO_IP4,
 	.layer		= PROTO_L3,
@@ -125,6 +143,7 @@ static const struct proto_ops ipv4_proto_ops = {
 	.field_changed  = ipv4_field_changed,
 	.packet_finish  = ipv4_packet_finish,
 	.set_next_proto = ipv4_set_next_proto,
+	.get_next_proto = ipv4_get_next_proto,
 };
 
 static struct proto_field ipv6_fields[] = {
diff --git a/trafgen_parser.y b/trafgen_parser.y
index 74015b5..38f170a 100644
--- a/trafgen_parser.y
+++ b/trafgen_parser.y
@@ -169,6 +169,8 @@ static inline void __setup_new_csum16(struct csum16 *s, off_t from, off_t to,
 
 struct packet *realloc_packet(void)
 {
+	uint32_t i;
+
 	if (test_ignore())
 		return NULL;
 
@@ -185,6 +187,9 @@ struct packet *realloc_packet(void)
 	__init_new_csum_slot(&packet_dyn[packetd_last]);
 	__init_new_fields_slot(&packet_dyn[packetd_last]);
 
+	for (i = 0; i < plen; i++)
+		packets[i].id = i;
+
 	return &packets[packet_last];
 }
 
diff --git a/trafgen_proto.c b/trafgen_proto.c
index 1d978e3..be2f8f4 100644
--- a/trafgen_proto.c
+++ b/trafgen_proto.c
@@ -35,6 +35,16 @@ struct packet *proto_hdr_packet(struct proto_hdr *hdr)
 	return packet_get(hdr->pkt_id);
 }
 
+struct proto_hdr *packet_last_header(struct packet *pkt)
+{
+	struct proto_hdr **headers = &pkt->headers[0];
+
+	if (pkt->headers_count == 0)
+		return NULL;
+
+	return headers[pkt->headers_count - 1];
+}
+
 struct proto_hdr *proto_lower_header(struct proto_hdr *hdr)
 {
 	struct packet *pkt = packet_get(hdr->pkt_id);
@@ -90,12 +100,18 @@ void proto_header_fields_add(struct proto_hdr *hdr,
 	struct proto_field *f;
 	int i;
 
-	if (!hdr->fields)
-		hdr->pkt_offset = pkt->len;
+	if (pkt->headers_count > 0) {
+		struct proto_hdr *last = packet_last_header(pkt);
+		bug_on(!last);
+
+		hdr->pkt_offset = last->pkt_offset + last->len;
+	}
 
 	proto_fields_realloc(hdr, hdr->fields_count + count);
 
 	for (i = 0; count >= 1; count--, i++) {
+		int fill_len;
+
 		f = &hdr->fields[hdr->fields_count - count];
 
 		f->id = fields[i].id;
@@ -109,9 +125,12 @@ void proto_header_fields_add(struct proto_hdr *hdr,
 		if (!f->len)
 			continue;
 
-		if (f->pkt_offset + f->len > pkt->len) {
+		fill_len = (f->pkt_offset + f->len) - (hdr->pkt_offset + hdr->len);
+		if (fill_len > 0) {
+			if (!pkt->is_created)
+				set_fill(0, (f->pkt_offset + f->len) - pkt->len);
+
 			hdr->len += f->len;
-			set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 		}
 	}
 }
@@ -133,9 +152,8 @@ bool proto_hdr_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 	return field ? field->is_set : false;
 }
 
-struct proto_hdr *proto_header_push(enum proto_id pid)
+struct proto_hdr *proto_packet_apply(enum proto_id pid, struct packet *pkt)
 {
-	struct packet *pkt = current_packet();
 	struct proto_hdr **headers = &pkt->headers[0];
 	const struct proto_ops *ops = proto_ops_by_id(pid);
 	struct proto_hdr *hdr;
@@ -144,7 +162,7 @@ struct proto_hdr *proto_header_push(enum proto_id pid)
 
 	hdr = xzmalloc(sizeof(*hdr));
 	hdr->ops = ops;
-	hdr->pkt_id = current_packet_id();
+	hdr->pkt_id = pkt->id;
 
 	if (ops && ops->header_init)
 		ops->header_init(hdr);
@@ -157,12 +175,25 @@ struct proto_hdr *proto_header_push(enum proto_id pid)
 	return hdr;
 }
 
+struct proto_hdr *proto_header_push(enum proto_id pid)
+{
+	return proto_packet_apply(pid, current_packet());
+}
+
 void proto_header_finish(struct proto_hdr *hdr)
 {
 	if (hdr && hdr->ops && hdr->ops->header_finish)
 		hdr->ops->header_finish(hdr);
 }
 
+enum proto_id proto_hdr_get_next_proto(struct proto_hdr *hdr)
+{
+	if (hdr->ops && hdr->ops->get_next_proto)
+		return hdr->ops->get_next_proto(hdr);
+
+	return __PROTO_MAX;
+}
+
 struct proto_hdr *proto_hdr_push_sub_header(struct proto_hdr *hdr, int id)
 {
 	struct proto_hdr *sub_hdr;
@@ -408,6 +439,13 @@ static uint8_t *__proto_field_get_bytes(struct proto_field *field)
 	return &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 }
 
+uint8_t *proto_hdr_field_get_bytes(struct proto_hdr *hdr, uint32_t fid)
+{
+	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
+
+	return __proto_field_get_bytes(field);
+}
+
 void proto_hdr_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 {
 	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 1);
@@ -447,6 +485,14 @@ uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 	return field_unmask_and_unshift(field, be32_to_cpu(val));
 }
 
+uint32_t proto_hdr_field_get_be32(struct proto_hdr *hdr, uint32_t fid)
+{
+	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
+	uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
+
+	return field_unmask_and_unshift(field, val);
+}
+
 void proto_hdr_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid,
 				       const uint8_t *bytes, size_t len)
 {
@@ -647,6 +693,13 @@ uint32_t proto_field_get_u32(struct proto_field *field)
 	return field_unmask_and_unshift(field, be32_to_cpu(val));
 }
 
+uint32_t proto_field_get_be32(struct proto_field *field)
+{
+	uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
+
+	return field_unmask_and_unshift(field, val);
+}
+
 void proto_field_set_be16(struct proto_field *field, uint16_t val)
 {
 	__proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
@@ -709,6 +762,8 @@ void proto_packet_finish(void)
 		if (ops && ops->packet_finish)
 			ops->packet_finish(hdr);
 	}
+
+	current_packet()->is_created = true;
 }
 
 static inline uint32_t field_inc(struct proto_field *field)
diff --git a/trafgen_proto.h b/trafgen_proto.h
index 36b8f2b..d756aca 100644
--- a/trafgen_proto.h
+++ b/trafgen_proto.h
@@ -49,6 +49,7 @@ struct proto_ops {
 	void (*packet_finish)(struct proto_hdr *hdr);
 	void (*packet_update)(struct proto_hdr *hdr);
 	void (*set_next_proto)(struct proto_hdr *hdr, enum proto_id pid);
+	enum proto_id (*get_next_proto)(struct proto_hdr *hdr);
 };
 
 struct proto_hdr {
@@ -101,11 +102,13 @@ struct proto_field {
 extern void protos_init(struct dev_io *dev);
 extern void proto_ops_register(const struct proto_ops *ops);
 
+struct proto_hdr *proto_packet_apply(enum proto_id pid, struct packet *pkt);
 extern struct proto_hdr *proto_header_push(enum proto_id pid);
 extern void proto_header_finish(struct proto_hdr *hdr);
 extern void proto_packet_finish(void);
 extern void proto_packet_update(uint32_t idx);
 
+extern enum proto_id proto_hdr_get_next_proto(struct proto_hdr *hdr);
 extern struct packet *proto_hdr_packet(struct proto_hdr *hdr);
 extern struct proto_hdr *proto_hdr_push_sub_header(struct proto_hdr *hdr, int id);
 extern void proto_hdr_move_sub_header(struct proto_hdr *hdr, struct proto_hdr *from,
@@ -114,6 +117,7 @@ extern void proto_hdr_move_sub_header(struct proto_hdr *hdr, struct proto_hdr *f
 extern struct proto_hdr *proto_lower_default_add(struct proto_hdr *hdr,
 						 enum proto_id pid);
 
+extern struct proto_hdr *packet_last_header(struct packet *pkt);
 extern struct proto_hdr *proto_lower_header(struct proto_hdr *hdr);
 extern struct proto_hdr *proto_upper_header(struct proto_hdr *hdr);
 extern uint8_t *proto_header_ptr(struct proto_hdr *hdr);
@@ -123,6 +127,7 @@ extern void proto_header_fields_add(struct proto_hdr *hdr,
 				    size_t count);
 
 extern bool proto_hdr_field_is_set(struct proto_hdr *hdr, uint32_t fid);
+extern uint8_t *proto_hdr_field_get_bytes(struct proto_hdr *hdr, uint32_t fid);
 extern void proto_hdr_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 				  const uint8_t *bytes, size_t len);
 extern void proto_hdr_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val);
@@ -131,6 +136,7 @@ extern void proto_hdr_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_
 extern uint16_t proto_hdr_field_get_u16(struct proto_hdr *hdr, uint32_t fid);
 extern void proto_hdr_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val);
 extern uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid);
+extern uint32_t proto_hdr_field_get_be32(struct proto_hdr *hdr, uint32_t fid);
 
 extern void proto_hdr_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid,
 					  const uint8_t *bytes, size_t len);
@@ -172,6 +178,7 @@ extern void proto_field_set_u16(struct proto_field *field, uint16_t val);
 extern uint16_t proto_field_get_u16(struct proto_field *field);
 extern void proto_field_set_u32(struct proto_field *field, uint32_t val);
 extern uint32_t proto_field_get_u32(struct proto_field *field);
+extern uint32_t proto_field_get_be32(struct proto_field *field);
 extern void proto_field_set_be16(struct proto_field *field, uint16_t val);
 extern void proto_field_set_be32(struct proto_field *field, uint32_t val);
 extern void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes, size_t len);
-- 
cgit v1.2.3-54-g00ecf