summaryrefslogtreecommitdiff
path: root/trafgen_l2.c
diff options
context:
space:
mode:
Diffstat (limited to 'trafgen_l2.c')
-rw-r--r--trafgen_l2.c63
1 files changed, 53 insertions, 10 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);
}