Split out construct_esp_packet() to avoid duplication

I want to make it set the next header field correctly, and that's the last straw; I don't want multiple copies of it. Signed-off-by: David Woodhouse <dwmw2@infradead.org>
sailfishos-mirror · Jun 9, 2019 · 931bdf1 · 931bdf1
1 parent 35e03f8
commit 931bdf1
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 43 deletions.
diff --git a/esp.c b/esp.c
@@ -97,6 +97,30 @@ int esp_setup(struct openconnect_info *vpninfo, int dtls_attempt_period)
 	return 0;
 }
 
+int construct_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt)
+{
+	const int blksize = 16;
+	int i, padlen, ret;
+
+	/* This gets much more fun if the IV is variable-length */
+	pkt->esp.spi = vpninfo->esp_out.spi;
+	pkt->esp.seq = htonl(vpninfo->esp_out.seq++);
+
+	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 */
+
+	memcpy(pkt->esp.iv, vpninfo->esp_out.iv, sizeof(pkt->esp.iv));
+
+	ret = encrypt_esp_packet(vpninfo, pkt, pkt->len + padlen + 2);
+	if (ret)
+		return ret;
+
+	return sizeof(pkt->esp) + pkt->len + padlen + 2 + vpninfo->hmac_out_len;
+}
+
 int esp_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable)
 {
 	struct esp *esp = &vpninfo->esp_in[vpninfo->current_esp_in];
@@ -275,11 +299,9 @@ int esp_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable)
 			if (!this)
 				break;
 
-			len = encrypt_esp_packet(vpninfo, this);
+			len = construct_esp_packet(vpninfo, this);
 			if (len < 0) {
-				vpn_progress(vpninfo, PRG_ERR,
-					     _("Failed to encrypt ESP packet: %d\n"),
-					     len);
+				/* Should we disable ESP? */
 				free(this);
 				work_done = 1;
 				continue;

diff --git a/gnutls-esp.c b/gnutls-esp.c
@@ -152,42 +152,29 @@ int decrypt_esp_packet(struct openconnect_info *vpninfo, struct esp *esp, struct
 	return 0;
 }
 
-int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt)
+int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt, int crypt_len)
 {
-	int i, padlen;
 	const int blksize = 16;
 	int err;
 
-	/* This gets much more fun if the IV is variable-length */
-	pkt->esp.spi = vpninfo->esp_out.spi;
-	pkt->esp.seq = htonl(vpninfo->esp_out.seq++);
-
-	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 */
-
-	memcpy(pkt->esp.iv, vpninfo->esp_out.iv, sizeof(pkt->esp.iv));
-
-	err = gnutls_cipher_encrypt(vpninfo->esp_out.cipher, pkt->data, pkt->len + padlen + 2);
+	err = gnutls_cipher_encrypt(vpninfo->esp_out.cipher, pkt->data, crypt_len);
 	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->esp, sizeof(pkt->esp) + pkt->len + padlen + 2);
+	err = gnutls_hmac(vpninfo->esp_out.hmac, &pkt->esp, sizeof(pkt->esp) + crypt_len);
 	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);
+	gnutls_hmac_output(vpninfo->esp_out.hmac, pkt->data + crypt_len);
 
-	memcpy(vpninfo->esp_out.iv, pkt->data + pkt->len + padlen + 2, blksize);
+	memcpy(vpninfo->esp_out.iv, pkt->data + crypt_len, blksize);
 	gnutls_cipher_encrypt(vpninfo->esp_out.cipher, vpninfo->esp_out.iv, blksize);
-	return sizeof(pkt->esp) + pkt->len + padlen + 2 + vpninfo->hmac_out_len;
+	return 0;
 }
diff --git a/gpst.c b/gpst.c
@@ -1286,7 +1286,7 @@ int gpst_esp_send_probes(struct openconnect_info *vpninfo)
 		memcpy(pmagic, magic_ping_payload, sizeof(magic_ping_payload)); /* required to get gateway to respond */
 		icmph->icmp_cksum = csum((uint16_t *)icmph, (ICMP_MINLEN+sizeof(magic_ping_payload))/2);
 
-		pktlen = encrypt_esp_packet(vpninfo, pkt);
+		pktlen = construct_esp_packet(vpninfo, pkt);
 		if (pktlen >= 0)
 			send(vpninfo->dtls_fd, (void *)&pkt->esp, pktlen, 0);
 	}

diff --git a/oncp.c b/oncp.c
@@ -1305,7 +1305,7 @@ int oncp_esp_send_probes(struct openconnect_info *vpninfo)
 	for (seq=1; seq <= (vpninfo->dtls_state==DTLS_CONNECTED ? 1 : 2); seq++) {
 		pkt->len = 1;
 		pkt->data[0] = 0;
-		pktlen = encrypt_esp_packet(vpninfo, pkt);
+		pktlen = construct_esp_packet(vpninfo, pkt);
 		if (pktlen >= 0)
 			send(vpninfo->dtls_fd, (void *)&pkt->esp, pktlen, 0);
 	}

diff --git a/openconnect-internal.h b/openconnect-internal.h
@@ -967,12 +967,13 @@ void esp_close(struct openconnect_info *vpninfo);
 void esp_shutdown(struct openconnect_info *vpninfo);
 int print_esp_keys(struct openconnect_info *vpninfo, const char *name, struct esp *esp);
 int openconnect_setup_esp_keys(struct openconnect_info *vpninfo, int new_keys);
+int construct_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt);
 
 /* {gnutls,openssl}-esp.c */
 void destroy_esp_ciphers(struct esp *esp);
 int init_esp_ciphers(struct openconnect_info *vpninfo, struct esp *out, struct esp *in);
 int decrypt_esp_packet(struct openconnect_info *vpninfo, struct esp *esp, struct pkt *pkt);
-int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt);
+int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt, int crypt_len);
 
 /* {gnutls,openssl}.c */
 int ssl_nonblock_read(struct openconnect_info *vpninfo, void *buf, int maxlen);

diff --git a/openssl-esp.c b/openssl-esp.c
@@ -189,26 +189,11 @@ int decrypt_esp_packet(struct openconnect_info *vpninfo, struct esp *esp, struct
 	return 0;
 }
 
-int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt)
+int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt, int crypt_len)
 {
-	int i, padlen;
 	int blksize = 16;
 	unsigned int hmac_len = vpninfo->hmac_out_len;
-	int crypt_len;
 
-	/* This gets much more fun if the IV is variable-length */
-	pkt->esp.spi = vpninfo->esp_out.spi;
-	pkt->esp.seq = htonl(vpninfo->esp_out.seq++);
-
-	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 */
-
-	memcpy(pkt->esp.iv, vpninfo->esp_out.iv, 16);
-
-	crypt_len = pkt->len + padlen + 2;
 	if (!EVP_EncryptUpdate(vpninfo->esp_out.cipher, pkt->data, &crypt_len,
 			       pkt->data, crypt_len)) {
 		vpn_progress(vpninfo, PRG_ERR,
@@ -223,5 +208,5 @@ int encrypt_esp_packet(struct openconnect_info *vpninfo, struct pkt *pkt)
 
 	EVP_EncryptUpdate(vpninfo->esp_out.cipher, vpninfo->esp_out.iv, &blksize,
 			  pkt->data + crypt_len + hmac_len - blksize, blksize);
-	return sizeof(pkt->esp) + crypt_len + vpninfo->hmac_out_len;
+	return 0;
 }