From 60da8a847801edf4043fcae012c6fb5499b6d803 Mon Sep 17 00:00:00 2001
From: David Woodhouse <David.Woodhouse@intel.com>
Date: Thu, 22 Jan 2015 02:13:58 +0000
Subject: [PATCH] Implement ESP encryption

Signed-off-by: David Woodhouse <David.Woodhouse@intel.com>
---
 gnutls-esp.c           | 49 ++++++++++++++++++++++++++++++++++++++++--
 mainloop.c             |  2 +-
 openconnect-internal.h | 17 ++++++++++++---
 3 files changed, 62 insertions(+), 6 deletions(-)

diff --git a/gnutls-esp.c b/gnutls-esp.c
index 06ad4c97..b6c1ef06 100644
--- a/gnutls-esp.c
+++ b/gnutls-esp.c
@@ -75,6 +75,7 @@ static int init_esp_ciphers(struct openconnect_info *vpninfo, struct esp *esp,
 			     gnutls_strerror(err));
 		destroy_esp_ciphers(esp);
 	}
+	esp->seq = 0;
 	return 0;
 }
 
@@ -109,7 +110,7 @@ int setup_esp_keys(struct openconnect_info *vpninfo)
 		return -EINVAL;
 	}
 
-	ret = gnutls_rnd(GNUTLS_RND_NONCE, &vpninfo->esp_in.spi,
+	ret = gnutls_rnd(GNUTLS_RND_RANDOM, &vpninfo->esp_in.spi,
 			 sizeof(vpninfo->esp_in.secrets) + sizeof(vpninfo->esp_in.spi));
 	if (ret) {
 		vpn_progress(vpninfo, PRG_ERR,
@@ -129,6 +130,7 @@ int setup_esp_keys(struct openconnect_info *vpninfo)
 	}
 
 	vpninfo->dtls_state = DTLS_SECRET;
+	vpninfo->pkt_trailer = 16 + 20; /* 16 for pad, 20 for HMAC (of which we use 16) */
 	return 0;
 }
 
@@ -137,7 +139,7 @@ int decrypt_and_queue_esp_packet(struct openconnect_info *vpninfo, unsigned char
 {
 	struct pkt *pkt;
 	unsigned char hmac_buf[20];
-	const int ivsize = 16; /* We don't support anything different... yet. */
+	const int ivsize = sizeof(pkt->esp.iv);
 
 	if (len < 20 + ivsize)
 		return -EINVAL;
@@ -193,3 +195,46 @@ int decrypt_and_queue_esp_packet(struct openconnect_info *vpninfo, unsigned char
 	queue_packet(&vpninfo->incoming_queue, pkt);
 	return 0;
 }
+
+int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt)
+{
+	int i, padlen;
+	const int blksize = 16;
+	int err;
+
+	/* This gets much more fun if the IV is variable-length */
+	memcpy(pkt->esp.spi, vpninfo->esp_out.spi, 4);
+	pkt->esp.seq = vpninfo->esp_out.seq++;
+	err = gnutls_rnd(GNUTLS_RND_RANDOM, pkt->esp.iv, sizeof(pkt->esp.iv));
+	if (err) {
+		vpn_progress(vpninfo, PRG_ERR,
+			     _("Failed to generate ESP packet IV: %s\n"),
+			     gnutls_strerror(err));
+		return -EIO;
+	}
+
+	padlen = blksize - 1 - ((pkt->len + 1) % blksize);
+	for (i=0; i<padlen; i++)
+		pkt->data[pkt->len + i] = i + 1;
+	pkt->data[pkt->len + padlen] = padlen;
+	pkt->data[pkt->len + padlen + 1] = 0x04; /* Legacy IP */
+	
+	gnutls_cipher_set_iv(vpninfo->esp_out.cipher, pkt->esp.iv, sizeof(pkt->esp.iv));
+	err = gnutls_cipher_encrypt(vpninfo->esp_out.cipher, pkt->data, pkt->len + padlen + 2);
+	if (err) {
+		vpn_progress(vpninfo, PRG_ERR,
+			     _("Failed to encrypt ESP packet: %s\n"),
+			     gnutls_strerror(err));
+		return -EIO;
+	}
+
+	err = gnutls_hmac(vpninfo->esp_out.hmac, pkt->data, pkt->len + padlen + 2);
+	if (err) {
+		vpn_progress(vpninfo, PRG_ERR,
+			     _("Failed to calculate HMAC for ESP packet: %s\n"),
+			     gnutls_strerror(err));
+		return -EIO;
+	}
+		gnutls_hmac_output(vpninfo->esp_out.hmac, pkt->data + pkt->len + padlen + 2);
+	return sizeof(pkt->esp) + pkt->len + padlen + 2 + 12;
+}
diff --git a/mainloop.c b/mainloop.c
index 4f0a4ba7..d2d24605 100644
--- a/mainloop.c
+++ b/mainloop.c
@@ -59,7 +59,7 @@ int tun_mainloop(struct openconnect_info *vpninfo, int *timeout)
 			int len = vpninfo->ip_info.mtu;
 
 			if (!out_pkt) {
-				out_pkt = malloc(sizeof(struct pkt) + len);
+				out_pkt = malloc(sizeof(struct pkt) + len + vpninfo->pkt_trailer);
 				if (!out_pkt) {
 					vpn_progress(vpninfo, PRG_ERR, _("Allocation failed\n"));
 					break;
diff --git a/openconnect-internal.h b/openconnect-internal.h
index c94e5e34..1560a076 100644
--- a/openconnect-internal.h
+++ b/openconnect-internal.h
@@ -122,9 +122,17 @@ struct pkt {
 	int len;
 	struct pkt *next;
 	union {
-		unsigned char oncp_hdr[22];
 		struct {
-			unsigned char pad[14];
+			unsigned char spi[4];
+			uint32_t seq;
+			unsigned char iv[16];
+		} esp;
+		struct {
+			unsigned char oncp_pad[2];
+			unsigned char oncp_hdr[22];
+		};
+		struct {
+			unsigned char pad[16];
 			unsigned char hdr[8];
 		};
 	};
@@ -244,6 +252,7 @@ struct esp {
 #else
 #error No OpenSSL support for ESP yet
 #endif
+	uint32_t seq;
 	unsigned char spi[4];
 	unsigned char secrets[0x40];
 };
@@ -431,7 +440,8 @@ struct openconnect_info {
 	struct pkt *cstp_pkt;
 	struct pkt *dtls_pkt;
 	struct pkt *tun_pkt;
-	
+	int pkt_trailer; /* How many bytes after payload for encryption (ESP HMAC) */
+
 	z_stream inflate_strm;
 	uint32_t inflate_adler32;
 	z_stream deflate_strm;
@@ -752,6 +762,7 @@ int load_pkcs11_certificate(struct openconnect_info *vpninfo);
 int setup_esp_keys(struct openconnect_info *vpninfo);
 void destroy_esp_ciphers(struct esp *esp);
 int decrypt_and_queue_esp_packet(struct openconnect_info *vpninfo, unsigned char *esp, int len);
+int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt);
 
 /* {gnutls,openssl}.c */
 int ssl_nonblock_read(struct openconnect_info *vpninfo, void *buf, int maxlen);