From 906cb247a831e37665a19e2c6b41b7cfd4c6621b Mon Sep 17 00:00:00 2001 From: Vadim Kochan Date: Thu, 1 Jun 2017 13:12:26 +0300 Subject: 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 Signed-off-by: Tobias Klauser --- trafgen_l2.c | 6 ++-- trafgen_parser.y | 4 +-- trafgen_proto.c | 102 +++++++++++++++++++++++++++++++++++++++++-------------- trafgen_proto.h | 7 ++-- 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); -- cgit v1.2.3-54-g00ecf