net-test: packetdrill: add PSP encryption

gtests/net/packetdrill/Makefile.common

@@ -20,7 +20,7 @@ packetdrill-lib := \
          symbols_openbsd.o \
          symbols_netbsd.o \
          gre_packet.o icmp_packet.o ip_packet.o tcp_packet.o udp_packet.o \
-         mpls_packet.o \
+         mpls_packet.o psp_packet.o \
          run.o run_command.o run_packet.o run_system_call.o \
          script.o socket.o system.o \
          tcp_options.o tcp_options_iterator.o tcp_options_to_string.o \

gtests/net/packetdrill/config.c

@@ -71,6 +71,7 @@ enum option_codes {
 	OPT_DRY_RUN,
 	OPT_IS_ANYIP,
 	OPT_SEND_OMIT_FREE,
+	OPT_PSP_UDP_DPORT,
 	OPT_DEBUG,
 	OPT_DEFINE = 'D',	/* a '-D' single-letter option */
 	OPT_VERBOSE = 'v',	/* a '-v' single-letter option */
@@ -110,6 +111,7 @@ struct option options[] = {
 	{ "dry_run",		.has_arg = false, NULL, OPT_DRY_RUN },
 	{ "is_anyip",		.has_arg = false, NULL, OPT_IS_ANYIP },
 	{ "send_omit_free",	.has_arg = false, NULL, OPT_SEND_OMIT_FREE },
+	{ "psp_udp_port",	.has_arg = true,  NULL, OPT_PSP_UDP_DPORT },
 	{ "debug",		.has_arg = false, NULL, OPT_DEBUG },
 	{ "define",		.has_arg = true,  NULL, OPT_DEFINE },
 	{ "verbose",		.has_arg = false, NULL, OPT_VERBOSE },
@@ -151,6 +153,7 @@ void show_usage(void)
 		"\t[--dry_run]\n"
 		"\t[--is_anyip]\n"
 		"\t[--send_omit_free]\n"
+		"\t[--psp_udp_port=<UDP destination port for parsing/building PSP packets>]\n"
 		"\t[--debug]\n"
 		"\t[--define symbol1=val1 --define symbol2=val2 ...]\n"
 		"\t[--verbose|-v]\n"
@@ -611,6 +614,12 @@ static void process_option(int opt, char *optarg, struct config *config,
 	case OPT_SEND_OMIT_FREE:
 		config->send_omit_free = true;
 		break;
+	case OPT_PSP_UDP_DPORT:
+		port = atoi(optarg);
+		if (port <= 0 || port > 0xffff)
+			die("%s: bad --psp_udp_port: %s\n", where, optarg);
+		config->psp_udp_port = port;
+		break;
 	case OPT_DEBUG:
 		opt_debug++;
 		break;

gtests/net/packetdrill/config.h

@@ -126,6 +126,7 @@ struct config {
 	int mtu;			/* MTU of tun device */
 
 	bool strict_segments;		/* check exact segmentation? */
+	int psp_udp_port;		/* UDP dst port for PSP encapsulation */
 
 	bool non_fatal_packet;		/* treat packet asserts as non-fatal */
 	bool non_fatal_syscall;		/* treat syscall asserts as non-fatal */

gtests/net/packetdrill/header.h

@@ -39,6 +39,7 @@
 #include "ip.h"
 #include "ipv6.h"
 #include "mpls.h"
+#include "psp.h"
 #include "tcp.h"
 #include "udp.h"
 
@@ -55,6 +56,7 @@ enum header_t {
 	HEADER_UDP,
 	HEADER_ICMPV4,
 	HEADER_ICMPV6,
+	HEADER_PSP,
 	HEADER_NUM_TYPES
 };
 
@@ -73,6 +75,7 @@ struct header {
 		struct udp *udp;
 		struct icmpv4 *icmpv4;
 		struct icmpv6 *icmpv6;
+		struct psp *psp;
 	} h;
 };
 

gtests/net/packetdrill/lexer.l

@@ -240,6 +240,11 @@ none			return NONE;
 checksum		return CHECKSUM;
 sequence#		return SEQUENCE;
 present			return PRESENT;
+encap			return ENCAP;
+psp			return PSP;
+spi			return SPI;
+vc			return VC;
+vnid			return VNID;
 mpls			return MPLS;
 label			return LABEL;
 tc			return TC;

gtests/net/packetdrill/packet.c

@@ -33,6 +33,8 @@
 #include "ip_packet.h"
 #include "logging.h"
 #include "mpls_packet.h"
+#include "psp_packet.h"
+#include "udp_packet.h"
 
 
 /* Info for all types of header we support. */
@@ -43,9 +45,10 @@ struct header_type_info header_types[HEADER_NUM_TYPES] = {
 	{ "GRE",    IPPROTO_GRE,	0,		gre_header_finish },
 	{ "MPLS",   0,			ETHERTYPE_MPLS_UC, mpls_header_finish },
 	{ "TCP",    IPPROTO_TCP,	0,		NULL },
-	{ "UDP",    IPPROTO_UDP,	0,		NULL },
+	{ "UDP",    IPPROTO_UDP,	0,		udp_header_finish },
 	{ "ICMPV4", IPPROTO_ICMP,	0,		NULL },
 	{ "ICMPV6", IPPROTO_ICMPV6,	0,		NULL },
+	{ "PSP",    0,			0,		psp_header_finish },
 };
 
 struct packet *packet_new(u32 buffer_bytes)
@@ -173,6 +176,7 @@ static void packet_duplicate_info(struct packet *packet,
 	packet->udp	= offset_ptr(old_base, new_base, old_packet->udp);
 	packet->icmpv4	= offset_ptr(old_base, new_base, old_packet->icmpv4);
 	packet->icmpv6	= offset_ptr(old_base, new_base, old_packet->icmpv6);
+	packet->psp	= offset_ptr(old_base, new_base, old_packet->psp);
 
 	packet->tcp_ts_val	= offset_ptr(old_base, new_base,
 					     old_packet->tcp_ts_val);
@@ -254,6 +258,11 @@ struct packet *packet_encapsulate(struct packet *outer, struct packet *inner)
 
 	packet_finish_encapsulation_headers(packet);
 
+	/* Both inner and outer may have PSP headers. Keep the inner. */
+	if (packet->psp == NULL)
+		packet->psp = offset_ptr(outer->buffer, packet->buffer,
+					 outer->psp);
+
 	packet->ip_bytes = outer->ip_bytes + inner->ip_bytes;
 
 	return packet;

gtests/net/packetdrill/packet.h

@@ -36,6 +36,7 @@
 #include "icmpv6.h"
 #include "ip.h"
 #include "ipv6.h"
+#include "psp.h"
 #include "tcp.h"
 #include "udp.h"
 #include "unaligned.h"
@@ -98,6 +99,8 @@ struct packet {
 	bool echoed_header;     /* icmp payload is an echoed header?
 				   This is for TCP/UDP */
 
+	/* Encapsulation */
+	struct psp *psp;
 
 	s64 time_usecs;		/* wall time of receive/send if non-zero */
 
@@ -201,7 +204,7 @@ static inline u8 *packet_start(const struct packet *packet)
 }
 
 /* Return a pointer to the first byte of the innermost IP header. */
-static inline u8 *ip_start(struct packet *packet)
+static inline u8 *ip_start(const struct packet *packet)
 {
 	if (packet->ipv4 != NULL)
 		return (u8 *)packet->ipv4;

gtests/net/packetdrill/packet_checksum.c

@@ -47,10 +47,13 @@ static void checksum_ipv4_packet(struct packet *packet)
 	/* Fill in IPv4-based layer 4 checksum. */
 	if (packet->tcp != NULL) {
 		struct tcp *tcp = packet->tcp;
+		const int encap_bytes = ((u8 *)tcp - (u8 *)ipv4) -
+					ipv4_header_len(ipv4);
+		const int tcp_bytes = l4_bytes - encap_bytes;
 		tcp->check = 0;
 		tcp->check = tcp_udp_v4_checksum(ipv4->src_ip,
 						 ipv4->dst_ip,
-						 IPPROTO_TCP, tcp, l4_bytes);
+						 IPPROTO_TCP, tcp, tcp_bytes);
 	} else if (packet->udp != NULL) {
 		struct udp *udp = packet->udp;
 		udp->check = 0;
@@ -81,10 +84,13 @@ static void checksum_ipv6_packet(struct packet *packet)
 	/* Fill in IPv6-based layer 4 checksum. */
 	if (packet->tcp != NULL) {
 		struct tcp *tcp = packet->tcp;
+		const int encap_bytes = ((u8 *)tcp - (u8 *)ipv6) -
+					sizeof(*ipv6);
+		const int tcp_bytes = l4_bytes - encap_bytes;
 		tcp->check = 0;
 		tcp->check = tcp_udp_v6_checksum(&ipv6->src_ip,
 						 &ipv6->dst_ip,
-						 IPPROTO_TCP, tcp, l4_bytes);
+						 IPPROTO_TCP, tcp, tcp_bytes);
 	} else if (packet->udp != NULL) {
 		struct udp *udp = packet->udp;
 		udp->check = 0;

gtests/net/packetdrill/packet_parser.c

@@ -42,6 +42,7 @@
 #include "ip_address.h"
 #include "logging.h"
 #include "packet.h"
+#include "psp_packet.h"
 #include "tcp.h"
 
 static int parse_ipv4(struct packet *packet, u8 *header_start, u8 *packet_end,
@@ -394,6 +395,52 @@ static int parse_tcp(struct packet *packet, u8 *layer4_start, int layer4_bytes,
 	return PACKET_BAD;
 }
 
+static int parse_psp(struct packet *packet, u8 *psp_start, int psp_bytes,
+		     u8 *packet_end, char **error)
+{
+	assert(psp_bytes >= 0);
+	assert(psp_start + psp_bytes <= packet_end);
+	if (psp_bytes < PSP_MINLEN) {
+		asprintf(error, "Truncated PSP header");
+		goto error_out;
+	}
+
+	struct psp *psp = (struct psp *)psp_start;
+	if (psp->version != 0) {
+		asprintf(error, "Unsupported PSP header version %u",
+			 psp->version);
+		goto error_out;
+	}
+	if (psp->ext_len != 1 + psp->has_vc) {
+		asprintf(error, "Wrong PSP header length");
+		goto error_out;
+	}
+
+	const int psp_header_len = psp_len(psp);
+	if (psp_header_len > psp_bytes) {
+		asprintf(error, "PSP header overflows packet");
+		goto error_out;
+	}
+
+	DEBUGP("PSP header len: %d\n", psp_header_len);
+
+	struct header *psp_header;
+	psp_header = packet_append_header(packet, HEADER_PSP, psp_header_len);
+	if (psp_header == NULL) {
+		asprintf(error, "Too many nested headers at PSP header");
+		goto error_out;
+	}
+	psp_header->total_bytes = psp_bytes;
+	packet->psp = psp;
+
+	u8 *next_proto_start = psp_start + psp_header_len;
+	return parse_layer4(packet, next_proto_start, psp->next_header,
+			    psp_bytes - psp_header_len, packet_end, error);
+
+error_out:
+	return PACKET_BAD;
+}
+
 /* Parse the UDP header. Return a packet_parse_result_t. */
 static int parse_udp(struct packet *packet, u8 *layer4_start, int layer4_bytes,
 		     u8 *packet_end, char **error)
@@ -406,8 +453,9 @@ static int parse_udp(struct packet *packet, u8 *layer4_start, int layer4_bytes,
 		asprintf(error, "Truncated UDP header");
 		goto error_out;
 	}
-	packet->udp = (struct udp *) p;
-	const int udp_len = ntohs(packet->udp->len);
+
+	struct udp *udp = (struct udp *)p;
+	const int udp_len = ntohs(udp->len);
 	const int udp_header_len = sizeof(struct udp);
 	if (udp_len < udp_header_len) {
 		asprintf(error, "UDP datagram length too small for UDP header");
@@ -432,8 +480,17 @@ static int parse_udp(struct packet *packet, u8 *layer4_start, int layer4_bytes,
 	p += layer4_bytes;
 	assert(p <= packet_end);
 
-	DEBUGP("UDP src port: %d\n", ntohs(packet->udp->src_port));
-	DEBUGP("UDP dst port: %d\n", ntohs(packet->udp->dst_port));
+	u16 dst_port = ntohs(udp->dst_port);
+	DEBUGP("UDP src port: %d\n", ntohs(udp->src_port));
+	DEBUGP("UDP dst port: %d\n", dst_port);
+
+	if (is_psp_port(dst_port))
+		return parse_psp(packet, layer4_start + udp_header_len,
+				 layer4_bytes - udp_header_len, packet_end,
+				 error);
+
+	/* This UDP header is the innermost L4 rather than part of an encap. */
+	packet->udp = udp;
 	return PACKET_OK;
 
 error_out: