trafgen: proto: Allow to set field with variable length

It is quite tricky to set field value with a variable length
(i.e. DNS query name), to make it possible the field
needs to be added to header with 'len=0' in that case there
will be no any payload allocation, but only while setting the field
value the packet will be appended with a real length bytes and after
the field needs to be relocated to the right place.

Also add 'len' parameter to *_set_bytes(...) functoins to have better
control over it.

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

4 files changed, 85 insertions, 34 deletions

diff --git a/trafgen_l2.c b/trafgen_l2.c
index 5fc0a0d..427ff9b 100644
--- a/trafgen_l2.c
+++ b/trafgen_l2.c
@@ -69,7 +69,7 @@ static void pause_header_init(struct proto_hdr *hdr)
 	struct proto_hdr *lower;
 
 	lower = proto_lower_default_add(hdr, PROTO_ETH);
-	proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, eth_dst);
+	proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, eth_dst, 6);
 
 	proto_header_fields_add(hdr, pause_fields, array_size(pause_fields));
 	proto_hdr_field_set_default_be16(hdr, PAUSE_OPCODE, 0x1);
@@ -109,7 +109,7 @@ static void pfc_header_init(struct proto_hdr *hdr)
 	struct proto_hdr *lower;
 
 	lower = proto_lower_default_add(hdr, PROTO_ETH);
-	proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, eth_dst);
+	proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, eth_dst, 6);
 
 	proto_header_fields_add(hdr, pfc_fields, array_size(pfc_fields));
 	proto_hdr_field_set_default_be16(hdr, PFC_OPCODE, 0x0101);
@@ -180,7 +180,7 @@ static void arp_header_init(struct proto_hdr *hdr)
 	if (lower->ops->id == PROTO_ETH) {
 		const uint8_t bcast[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
-		proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, bcast);
+		proto_hdr_field_set_default_bytes(lower, ETH_DST_ADDR, bcast, 6);
 	}
 
 	proto_header_fields_add(hdr, arp_fields, array_size(arp_fields));
diff --git a/trafgen_parser.y b/trafgen_parser.y
index 5a61a3f..b4eedea 100644
--- a/trafgen_parser.y
+++ b/trafgen_parser.y
@@ -433,11 +433,11 @@ static void proto_field_expr_eval(void)
 		else
 			panic("Invalid value length %zu, can be 1,2 or 4\n", field->len);
 	} else if (field_expr.type & FIELD_EXPR_MAC) {
-		proto_field_set_bytes(field, field_expr.val.mac);
+		proto_field_set_bytes(field, field_expr.val.mac, 6);
 	} else if (field_expr.type & FIELD_EXPR_IP4_ADDR) {
 		proto_field_set_u32(field, field_expr.val.ip4_addr.s_addr);
 	} else if (field_expr.type & FIELD_EXPR_IP6_ADDR) {
-		proto_field_set_bytes(field, (uint8_t *)&field_expr.val.ip6_addr.s6_addr);
+		proto_field_set_bytes(field, (uint8_t *)&field_expr.val.ip6_addr.s6_addr, 16);
 	} else if ((field_expr.type & FIELD_EXPR_INC) ||
 			(field_expr.type & FIELD_EXPR_RND)) {
 
diff --git a/trafgen_proto.c b/trafgen_proto.c
index b802a3a..7969b77 100644
--- a/trafgen_proto.c
+++ b/trafgen_proto.c
@@ -100,6 +100,9 @@ void proto_header_fields_add(struct proto_hdr *hdr,
 		f->pkt_offset = hdr->pkt_offset + fields[i].offset;
 		f->hdr = hdr;
 
+		if (!f->len)
+			continue;
+
 		if (f->pkt_offset + f->len > pkt->len) {
 			hdr->len += f->len;
 			set_fill(0, (f->pkt_offset + f->len) - pkt->len);
@@ -181,9 +184,48 @@ set_proto:
 	return current;
 }
 
+static void __proto_field_relocate(struct proto_field *field)
+{
+	struct proto_hdr *hdr = field->hdr;
+	struct packet *pkt = packet_get(hdr->pkt_id);
+	uint8_t *from, *to;
+	int i;
+
+	/* If this is a last field then just calculate 'pkt_offset' */
+	if (field->id == hdr->fields_count - 1) {
+		field->pkt_offset = hdr->pkt_offset + hdr->len - field->len;
+		return;
+	}
+
+	/* Use 'pkt_offset' from the 1st real (len > 0) field after the
+	 * 'target' one */
+	for (i = field->id + 1; i < hdr->fields_count; i++) {
+		if (hdr->fields[i].len == 0)
+			continue;
+
+		field->pkt_offset = hdr->fields[i].pkt_offset;
+		break;
+	}
+
+	/* Move payload of overlapped fields (each after the 'target' field) */
+	from = &pkt->payload[field->pkt_offset];
+	to = &pkt->payload[field->pkt_offset + field->len];
+	memcpy(to, from, hdr->len - field->len);
+
+	/* Recalculate 'pkt_offset' of the rest fields */
+	for (; i < hdr->fields_count; i++) {
+		struct proto_field *tmp = &hdr->fields[i];
+
+		if (tmp->len == 0)
+			continue;
+
+		tmp->pkt_offset += field->len;
+	}
+}
+
 static void __proto_field_set_bytes(struct proto_field *field,
-				    const uint8_t *bytes, bool is_default,
-				    bool is_be)
+				    const uint8_t *bytes, size_t len,
+				    bool is_default, bool is_be)
 {
 	uint8_t *payload, *p8;
 	uint16_t *p16;
@@ -195,6 +237,14 @@ static void __proto_field_set_bytes(struct proto_field *field,
 	if (is_default && field->is_set)
 		return;
 
+	if (field->len == 0) {
+		field->hdr->len += len;
+		field->len = len;
+		set_fill(0, len);
+
+		__proto_field_relocate(field);
+	}
+
 	payload = &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 
 	if (field->len == 1) {
@@ -234,11 +284,11 @@ static void __proto_field_set_bytes(struct proto_field *field,
 }
 
 void proto_hdr_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
-			       const uint8_t *bytes)
+			       const uint8_t *bytes, size_t len)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, bytes, false, false);
+	__proto_field_set_bytes(field, bytes, len, false, false);
 }
 
 static uint8_t *__proto_field_get_bytes(struct proto_field *field)
@@ -248,7 +298,7 @@ static uint8_t *__proto_field_get_bytes(struct proto_field *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);
+	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 1);
 }
 
 uint8_t proto_hdr_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
@@ -261,7 +311,7 @@ uint8_t proto_hdr_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
 
 void proto_hdr_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 {
-	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val);
+	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 2);
 }
 
 uint16_t proto_hdr_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
@@ -274,7 +324,7 @@ uint16_t proto_hdr_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
 
 void proto_hdr_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 {
-	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val);
+	proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 4);
 }
 
 uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
@@ -286,60 +336,60 @@ uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 }
 
 void proto_hdr_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid,
-				       const uint8_t *bytes)
+				       const uint8_t *bytes, size_t len)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, bytes, true, false);
+	__proto_field_set_bytes(field, bytes, len, true, false);
 }
 
 void proto_hdr_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, true, false);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 1, true, false);
 }
 
 void proto_hdr_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, true, false);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 2, true, false);
 }
 
 void proto_hdr_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, true, false);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 4, true, false);
 }
 
 void proto_hdr_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
 }
 
 void proto_hdr_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 4, false, true);
 }
 
 void proto_hdr_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, true, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 2, true, true);
 }
 
 void proto_hdr_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 {
 	struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 
-	__proto_field_set_bytes(field, (uint8_t *)&val, true, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 4, true, true);
 }
 
 static void __proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
@@ -356,7 +406,7 @@ static void __proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 	if (ret < 0)
 		panic("Could not get device hw address\n");
 
-	__proto_field_set_bytes(field, mac, is_default, false);
+	__proto_field_set_bytes(field, mac, 6, is_default, false);
 }
 
 void proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
@@ -387,7 +437,7 @@ static void __proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 	}
 
 	ss4 = (struct sockaddr_in *) &ss;
-	__proto_field_set_bytes(field, (uint8_t *)&ss4->sin_addr.s_addr, is_default, false);
+	__proto_field_set_bytes(field, (uint8_t *)&ss4->sin_addr.s_addr, 4, is_default, false);
 }
 
 void proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
@@ -418,7 +468,7 @@ static void __proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 	}
 
 	ss6 = (struct sockaddr_in6 *) &ss;
-	__proto_field_set_bytes(field, (uint8_t *)&ss6->sin6_addr.s6_addr, is_default, false);
+	__proto_field_set_bytes(field, (uint8_t *)&ss6->sin6_addr.s6_addr, 16, is_default, false);
 }
 
 void proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
@@ -433,7 +483,7 @@ void proto_hdr_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 
 void proto_field_set_u8(struct proto_field *field, uint8_t val)
 {
-	__proto_field_set_bytes(field, &val, false, false);
+	__proto_field_set_bytes(field, &val, 1, false, false);
 }
 
 uint8_t proto_field_get_u8(struct proto_field *field)
@@ -445,7 +495,7 @@ uint8_t proto_field_get_u8(struct proto_field *field)
 
 void proto_field_set_u16(struct proto_field *field, uint16_t val)
 {
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, false);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 2, false, false);
 }
 
 uint16_t proto_field_get_u16(struct proto_field *field)
@@ -457,7 +507,7 @@ uint16_t proto_field_get_u16(struct proto_field *field)
 
 void proto_field_set_u32(struct proto_field *field, uint32_t val)
 {
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, false);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 4, false, false);
 }
 
 uint32_t proto_field_get_u32(struct proto_field *field)
@@ -469,17 +519,17 @@ uint32_t proto_field_get_u32(struct proto_field *field)
 
 void proto_field_set_be16(struct proto_field *field, uint16_t val)
 {
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
 }
 
 void proto_field_set_be32(struct proto_field *field, uint32_t val)
 {
-	__proto_field_set_bytes(field, (uint8_t *)&val, false, true);
+	__proto_field_set_bytes(field, (uint8_t *)&val, 4, false, true);
 }
 
-void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes)
+void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes, size_t len)
 {
-	__proto_field_set_bytes(field, bytes, false, false);
+	__proto_field_set_bytes(field, bytes, len, false, false);
 }
 
 void protos_init(const char *dev)
diff --git a/trafgen_proto.h b/trafgen_proto.h
index 56e7271..94eb4e7 100644
--- a/trafgen_proto.h
+++ b/trafgen_proto.h
@@ -108,7 +108,7 @@ extern void proto_header_fields_add(struct proto_hdr *hdr,
 
 extern bool proto_hdr_field_is_set(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);
+				  const uint8_t *bytes, size_t len);
 extern void proto_hdr_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val);
 extern uint8_t proto_hdr_field_get_u8(struct proto_hdr *hdr, uint32_t fid);
 extern void proto_hdr_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val);
@@ -117,7 +117,7 @@ extern void proto_hdr_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_
 extern uint32_t proto_hdr_field_get_u32(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);
+					  const uint8_t *bytes, size_t len);
 extern void proto_hdr_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid,
 				       uint8_t val);
 extern void proto_hdr_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid,
@@ -155,7 +155,8 @@ 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 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);
+extern void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes,
+				  size_t len);
 
 extern void proto_field_func_add(struct proto_field *field,
 				 struct proto_field_func *func);