diff options
| -rw-r--r-- | trafgen_l2.c | 63 | ||||
| -rw-r--r-- | trafgen_l2.h | 9 | ||||
| -rw-r--r-- | 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(ð_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; |