feat: implement decap_packet lib function

lib/dataplane/packet/decap.c

@@ -1,27 +1,105 @@
+#include <netinet/in.h>
+#include <rte_ether.h>
+#include <rte_gre.h>
+
 #include "decap.h"
+#include "packet.h"
 
-#include <string.h>
+// (first u32 + count of optional u32) * 4
+#define decap_gre_header_size(byte0)                                           \
+	(1 + __builtin_popcount((byte0) & 0xb0)) << 2
 
-#include <rte_ether.h>
+/// GRE Header:
+///                      1 1 1 1 1 1 1 1 1 1 2 2 2 2 2 2 2 2 2 2 3 3
+///  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+/// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+/// |C| |K|S| Reserved0       | Ver |         Protocol Type         |
+/// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+/// |      Checksum (optional)      |       Reserved1 (Optional)    |
+/// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+/// |                         Key (optional)                        |
+/// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+/// |                 Sequence Number (optional)                    |
+/// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+/// https://datatracker.ietf.org/doc/html/rfc2890#section-2
+static int
+packet_skip_gre(
+	struct packet *packet,
+	uint16_t *type,
+	uint16_t *offset,
+	uint16_t *next_proto
+) {
+	struct rte_gre_hdr *gre_hdr = rte_pktmbuf_mtod_offset(
+		packet->mbuf,
+		struct rte_gre_hdr *,
+		packet->transport_header.offset
+	);
+	*next_proto = gre_hdr->proto;
+	if (gre_hdr->proto == rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV4)) {
+		*type = IPPROTO_IPIP;
+	} else if (gre_hdr->proto == rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV6)) {
+		*type = IPPROTO_IPV6;
+	} else { // error case
+		return -1;
+	}
+	*offset += decap_gre_header_size(*(uint8_t *)gre_hdr);
+	return 0;
+}
 
 int
 packet_decap(struct packet *packet) {
-	(void)packet;
-
-	/*
-		if (packet->network_header.type ==
-		    rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV4)) {
-			struct rte_ipv4_hdr *ipv4Header =
-	   rte_pktmbuf_mtod_offset( mbuf, struct rte_ipv4_hdr *,
-				packet->network_header.offset
-			);
-		    if (ipv4Header->proto = IPPROTO_IPIP) {
-
-		    }
-		} else {
+	uint16_t next_transport = packet->transport_header.type;
+	uint16_t next_offset = packet->transport_header.offset;
+	uint16_t next_ether_type;
+
+	if (next_transport == IPPROTO_GRE) {
+		if (packet_skip_gre(
+			    packet,
+			    &next_transport,
+			    &next_offset,
+			    &next_ether_type
+		    )) {
 			return -1;
 		}
-	*/
+	}
+	uint16_t tun_hdrs_size = next_offset - packet->network_header.offset;
+
+	if (next_transport == IPPROTO_IPIP) {
+		if (parse_ipv4_header(packet, &next_transport, &next_offset)) {
+			return -1;
+		}
+	} else if (next_transport == IPPROTO_IPV6) {
+		if (parse_ipv6_header(packet, &next_transport, &next_offset)) {
+			return -1;
+		}
+	} else {
+		// unknown tunnel
+		return -1;
+	}
+
+	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
+
+	// Remove tunnel headers
+	uint8_t *prev_start = rte_pktmbuf_mtod(mbuf, uint8_t *);
+
+	// FIXME: separate strip header function
+	// FIXME: handle rte_pktmbuf_adj() == NULL
+	/* Copy ether header over rather than moving whole packet */
+	memmove(rte_pktmbuf_adj(mbuf, tun_hdrs_size),
+		prev_start,
+		// size of eth_hdr plus optional vlans
+		packet->network_header.offset);
+
+	// Update ether type
+	uint16_t *prev_eth_type = rte_pktmbuf_mtod_offset(
+		mbuf,
+		uint16_t *,
+		packet->network_header.offset - sizeof(uint16_t)
+	);
+	*prev_eth_type = next_ether_type;
+	// and transport_header meta
+	packet->transport_header.type = next_transport;
+	packet->transport_header.offset = next_offset - tun_hdrs_size;
 
 	return 0;
 }

lib/dataplane/packet/packet.c

@@ -20,9 +20,9 @@ parse_ether_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 		return -1;
 	}
 
-	const struct rte_ether_hdr *etherHeader =
+	const struct rte_ether_hdr *ether_hdr =
 		rte_pktmbuf_mtod_offset(mbuf, struct rte_ether_hdr *, *offset);
-	*type = etherHeader->ether_type;
+	*type = ether_hdr->ether_type;
 	*offset += sizeof(struct rte_ether_hdr);
 	return 0;
 }
@@ -37,17 +37,17 @@ parse_vlan_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 		return -1;
 	}
 
-	const struct rte_vlan_hdr *vlanHeader =
+	const struct rte_vlan_hdr *vlan_hdr =
 		rte_pktmbuf_mtod_offset(mbuf, struct rte_vlan_hdr *, *offset);
 
-	packet->vlan = rte_be_to_cpu_16(vlanHeader->vlan_tci);
+	packet->vlan = rte_be_to_cpu_16(vlan_hdr->vlan_tci);
 
-	*type = vlanHeader->eth_proto;
+	*type = vlan_hdr->eth_proto;
 	*offset += sizeof(struct rte_vlan_hdr);
 	return 0;
 }
 
-static inline int
+inline int
 parse_ipv4_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
 
@@ -57,36 +57,36 @@ parse_ipv4_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 		return -1;
 	}
 
-	const struct rte_ipv4_hdr *ipv4Header =
+	const struct rte_ipv4_hdr *ipv4_hdr =
 		rte_pktmbuf_mtod_offset(mbuf, struct rte_ipv4_hdr *, *offset);
 
 	if (rte_pktmbuf_pkt_len(mbuf) <
-	    (uint32_t)*offset + rte_be_to_cpu_16(ipv4Header->total_length)) {
+	    (uint32_t)*offset + rte_be_to_cpu_16(ipv4_hdr->total_length)) {
 		*type = PACKET_HEADER_TYPE_UNKNOWN;
 		return -1;
 	}
 
-	if ((ipv4Header->version_ihl & 0x0F) < 0x05) {
+	if ((ipv4_hdr->version_ihl & 0x0F) < 0x05) {
 		*type = PACKET_HEADER_TYPE_UNKNOWN;
 		return -1;
 	}
 
-	if (rte_be_to_cpu_16(ipv4Header->total_length) <
-	    4 * (ipv4Header->version_ihl & 0x0F)) {
+	if (rte_be_to_cpu_16(ipv4_hdr->total_length) <
+	    4 * (ipv4_hdr->version_ihl & 0x0F)) {
 		*type = PACKET_HEADER_TYPE_UNKNOWN;
 		return -1;
 	}
 
 	// FIXME: check if fragmented
 	// FIXME: process extensions
 
-	*type = ipv4Header->next_proto_id;
-	*offset += 4 * (ipv4Header->version_ihl & 0x0F);
+	*type = ipv4_hdr->next_proto_id;
+	*offset += 4 * (ipv4_hdr->version_ihl & 0x0F);
 
 	return 0;
 }
 
-static inline int
+inline int
 parse_ipv6_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
 
@@ -96,21 +96,21 @@ parse_ipv6_header(struct packet *packet, uint16_t *type, uint16_t *offset) {
 		return -1;
 	}
 
-	const struct rte_ipv6_hdr *ipv6Header =
+	const struct rte_ipv6_hdr *ipv6_hdr =
 		rte_pktmbuf_mtod_offset(mbuf, struct rte_ipv6_hdr *, *offset);
 
 	if (rte_pktmbuf_pkt_len(mbuf) <
 	    *offset + sizeof(struct rte_ipv6_hdr) +
-		    rte_be_to_cpu_16(ipv6Header->payload_len)) {
+		    rte_be_to_cpu_16(ipv6_hdr->payload_len)) {
 		*type = PACKET_HEADER_TYPE_UNKNOWN;
 		return -1;
 	}
 
 	// walk through extensions
 	*offset += sizeof(struct rte_ipv6_hdr);
 	uint16_t max_offset =
-		*offset + rte_be_to_cpu_16(ipv6Header->payload_len);
-	uint8_t ext_type = ipv6Header->proto;
+		*offset + rte_be_to_cpu_16(ipv6_hdr->payload_len);
+	uint8_t ext_type = ipv6_hdr->proto;
 	while (*offset < max_offset) {
 		if (ext_type == IPPROTO_HOPOPTS ||
 		    ext_type == IPPROTO_ROUTING ||

lib/dataplane/packet/packet.h

@@ -99,6 +99,12 @@ packet_list_pop(struct packet_list *packets) {
 	return res;
 }
 
+int
+parse_ipv4_header(struct packet *packet, uint16_t *type, uint16_t *offset);
+
+int
+parse_ipv6_header(struct packet *packet, uint16_t *type, uint16_t *offset);
+
 int
 parse_packet(struct packet *packet);
 

modules/decap/dataplane.c

@@ -1,7 +1,5 @@
 #include "dataplane.h"
 
-#include "dataplane/pipeline/pipeline.h"
-
 #include "dataplane/packet/decap.h"
 
 #include "rte_ether.h"
@@ -11,11 +9,11 @@ static int
 decap_handle_v4(const struct lpm *lpm, struct packet *packet) {
 	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
 
-	struct rte_ipv4_hdr *ipv4Header = rte_pktmbuf_mtod_offset(
+	struct rte_ipv4_hdr *ipv4_hdr = rte_pktmbuf_mtod_offset(
 		mbuf, struct rte_ipv4_hdr *, packet->network_header.offset
 	);
 
-	if (lpm_lookup(lpm, 4, (uint8_t *)&ipv4Header->dst_addr) !=
+	if (lpm_lookup(lpm, 4, (uint8_t *)&ipv4_hdr->dst_addr) !=
 	    LPM_VALUE_INVALID) {
 		return packet_decap(packet);
 	}
@@ -27,12 +25,13 @@ static int
 decap_handle_v6(const struct lpm *lpm, struct packet *packet) {
 	struct rte_mbuf *mbuf = packet_to_mbuf(packet);
 
-	struct rte_ipv6_hdr *ipv4Header = rte_pktmbuf_mtod_offset(
+	struct rte_ipv6_hdr *ipv6_hdr = rte_pktmbuf_mtod_offset(
 		mbuf, struct rte_ipv6_hdr *, packet->network_header.offset
 	);
 
-	if (lpm_lookup(lpm, 16, (uint8_t *)&ipv4Header->dst_addr) !=
+	if (lpm_lookup(lpm, 16, (uint8_t *)&ipv6_hdr->dst_addr) !=
 	    LPM_VALUE_INVALID) {
+		// FIXME: preserve flowlabel
 		return packet_decap(packet);
 	}
 
@@ -46,6 +45,7 @@ decap_handle_packets(
 	struct packet_front *packet_front
 ) {
 	(void)module;
+
 	struct decap_module_config *decap_config =
 		container_of(config, struct decap_module_config, config);
 
@@ -67,6 +67,9 @@ decap_handle_packets(
 		if (result) {
 			packet_front_drop(packet_front, packet);
 		} else {
+			// FIXME: if packet is ipv4 then mark ipv4 dscp
+			// FIXME: if packet is ipv6 restore flowlabel
+
 			packet_front_output(packet_front, packet);
 		}
 	}
@@ -80,9 +83,6 @@ decap_handle_configure(
 	struct module_config **new_config
 ) {
 	(void)module;
-	(void)config_data;
-	(void)config_data_size;
-	(void)new_config;
 
 	struct decap_module_config *config = (struct decap_module_config *)
 		malloc(sizeof(struct decap_module_config));