From a4e17af38e2e2545c32292b7b46102d49e738e1e Mon Sep 17 00:00:00 2001
From: Vadim Kochan <vadim4j@gmail.com>
Date: Mon, 1 Feb 2016 19:01:39 +0200
Subject: trafgen: l2: Add VLAN header generation

Add generation of VLAN header with supported fields:

    TPID, DEI/CFI, PCP, VID

Changed struct proto_field.offset uint16_t -> int16_t to make
TPID offset -2 to point to ether type.

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
Signed-off-by: Tobias Klauser <tklauser@distanz.ch>
---
 trafgen_l2.c    | 63 ++++++++++++++++++++++++++++++++++++++++++++++++---------
 trafgen_l2.h    |  9 +++++++++
 trafgen_proto.h |  4 +++-
 3 files changed, 65 insertions(+), 11 deletions(-)

diff --git a/trafgen_l2.c b/trafgen_l2.c
index c0e92a3..a7387b8 100644
--- a/trafgen_l2.c
+++ b/trafgen_l2.c
@@ -17,25 +17,25 @@ static struct proto_field eth_fields[] = {
 	{ .id = ETH_TYPE,     .len = 2, .offset = 12 },
 };
 
-static void eth_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
+static uint16_t pid_to_eth(enum proto_id pid)
 {
-	uint16_t eth_type;
-
 	switch(pid) {
 	case PROTO_ARP:
-		eth_type = ETH_P_ARP;
-		break;
+		return ETH_P_ARP;
 	case PROTO_IP4:
-		eth_type = ETH_P_IP;
-		break;
+		return ETH_P_IP;
 	case PROTO_IP6:
-		eth_type = ETH_P_IPV6;
-		break;
+		return ETH_P_IPV6;
+	case PROTO_VLAN:
+		return ETH_P_8021Q;
 	default:
 		panic("eth: Not supported protocol id %u\n", pid);
 	}
+}
 
-	proto_field_set_default_be16(hdr, ETH_TYPE, eth_type);
+static void eth_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
+{
+	proto_field_set_default_be16(hdr, ETH_TYPE, pid_to_eth(pid));
 }
 
 static void eth_header_init(struct proto_hdr *hdr)
@@ -52,6 +52,48 @@ static struct proto_hdr eth_hdr = {
 	.set_next_proto = eth_set_next_proto,
 };
 
+static struct proto_field vlan_fields[] = {
+	/* TPID overlaps with Ethernet header and points to ether type */
+	{ .id = VLAN_TPID, .len = 2, .offset = -2 },
+	{ .id = VLAN_TCI,  .len = 2, .offset = 0 },
+	{ .id = VLAN_PCP,  .len = 2, .offset = 0, .shift = 13, .mask = 0xe000 },
+	{ .id = VLAN_DEI,  .len = 2, .offset = 0, .shift = 12, .mask = 0x1000 },
+	{ .id = VLAN_VID,  .len = 2, .offset = 0, .shift = 0,  .mask = 0xfff },
+	/* Original ether type is stored after VLAN header */
+	{ .id = VLAN_ETYPE, .len = 2, .offset = 2 },
+};
+
+static void vlan_header_init(struct proto_hdr *hdr)
+{
+	struct proto_hdr *lower;
+	uint16_t lower_etype = 0;
+
+	lower = proto_lower_default_add(hdr, PROTO_ETH);
+
+	proto_header_fields_add(hdr, vlan_fields, array_size(vlan_fields));
+
+	if (lower->id == PROTO_ETH)
+		lower_etype = proto_field_get_u16(lower, ETH_TYPE);
+	else if (lower->id == PROTO_VLAN)
+		lower_etype = proto_field_get_u16(lower, VLAN_ETYPE);
+
+	proto_field_set_be16(hdr, VLAN_ETYPE, lower_etype);
+	proto_field_set_default_be16(hdr, VLAN_TPID, pid_to_eth(hdr->id));
+}
+
+static void vlan_set_next_proto(struct proto_hdr *hdr, enum proto_id pid)
+{
+	if (pid != PROTO_VLAN)
+		proto_field_set_be16(hdr, VLAN_ETYPE, pid_to_eth(pid));
+}
+
+static struct proto_hdr vlan_hdr = {
+	.id		= PROTO_VLAN,
+	.layer		= PROTO_L2,
+	.header_init	= vlan_header_init,
+	.set_next_proto = vlan_set_next_proto,
+};
+
 static struct proto_field arp_fields[] = {
 	{ .id = ARP_HTYPE, .len = 2 },
 	{ .id = ARP_PTYPE, .len = 2, .offset = 2 },
@@ -98,5 +140,6 @@ static struct proto_hdr arp_hdr = {
 void protos_l2_init(void)
 {
 	proto_header_register(&eth_hdr);
+	proto_header_register(&vlan_hdr);
 	proto_header_register(&arp_hdr);
 }
diff --git a/trafgen_l2.h b/trafgen_l2.h
index 38727ec..9beb2aa 100644
--- a/trafgen_l2.h
+++ b/trafgen_l2.h
@@ -19,6 +19,15 @@ enum arp_field {
 	ARP_TPA,
 };
 
+enum vlan_field {
+	VLAN_TPID,
+	VLAN_TCI,
+	VLAN_PCP,
+	VLAN_DEI,
+	VLAN_VID,
+	VLAN_ETYPE,
+};
+
 extern void protos_l2_init(void);
 
 #endif /* TRAFGEN_L2_H */
diff --git a/trafgen_proto.h b/trafgen_proto.h
index 491e079..996c24e 100644
--- a/trafgen_proto.h
+++ b/trafgen_proto.h
@@ -12,6 +12,7 @@ struct proto_ctx {
 enum proto_id {
 	PROTO_NONE,
 	PROTO_ETH,
+	PROTO_VLAN,
 	PROTO_ARP,
 	PROTO_IP4,
 	PROTO_IP6,
@@ -31,7 +32,8 @@ struct proto_field {
 	size_t len;
 	uint32_t shift;
 	uint32_t mask;
-	uint16_t offset;
+	/* might be negative (e.g. VLAN TPID field) */
+	int16_t offset;
 
 	bool is_set;
 	uint16_t pkt_offset;
-- 
cgit v1.2.3-54-g00ecf