diff --git a/dtls.c b/dtls.c
index 0ebd639d..ca68fede 100644
--- a/dtls.c
+++ b/dtls.c
@@ -242,6 +242,38 @@ int dtls_setup(struct openconnect_info *vpninfo, int dtls_attempt_period)
 	return 0;
 }
 
+int udp_tos_update(struct openconnect_info *vpninfo, struct pkt *pkt)
+{
+	int tos;
+
+	/* Extract TOS field from IP header (IPv4 and IPv6 differ) */
+	switch(pkt->data[0] >> 4) {
+		case 4:
+			tos = pkt->data[1];
+			break;
+		case 6:
+			tos = (load_be16(pkt->data) >> 4) & 0xff;
+			break;
+		default:
+			vpn_progress(vpninfo, PRG_ERR,
+				     _("Unknown packet (len %d) received: %02x %02x %02x %02x...\n"),
+				     pkt->len, pkt->data[0], pkt->data[1], pkt->data[2], pkt->data[3]);
+			return -EINVAL;
+	}
+
+	/* set the actual value */
+	if (tos != vpninfo->dtls_tos_current) {
+		vpn_progress(vpninfo, PRG_DEBUG, _("TOS this: %d, TOS last: %d\n"),
+			     tos, vpninfo->dtls_tos_current);
+		if (setsockopt(vpninfo->dtls_fd, vpninfo->dtls_tos_proto,
+			       vpninfo->dtls_tos_optname, (void *)&tos, sizeof(tos)))
+			vpn_perror(vpninfo, _("UDP setsockopt"));
+		else
+			vpninfo->dtls_tos_current = tos;
+	}
+	return 0;
+}
+
 int dtls_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable)
 {
 	int work_done = 0;
@@ -416,35 +448,8 @@ int dtls_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable)
 
 		/* If TOS optname is set, we want to copy the TOS/TCLASS header
 		   to the outer UDP packet */
-		if (vpninfo->dtls_tos_optname) {
-			int valid=1;
-			int tos;
-
-			switch(this->data[0] >> 4) {
-				case 4:
-					tos = this->data[1];
-					break;
-				case 6:
-					tos = (load_be16(this->data) >> 4) & 0xff;
-					break;
-				default:
-					vpn_progress(vpninfo, PRG_ERR,
-						     _("Unknown packet (len %d) received: %02x %02x %02x %02x...\n"),
-						     this->len, this->data[0], this->data[1], this->data[2], this->data[3]);
-					valid = 0;
-			}
-
-			/* set the actual value */
-			if (valid && tos != vpninfo->dtls_tos_current) {
-				vpn_progress(vpninfo, PRG_DEBUG, _("TOS this: %d, TOS last: %d\n"),
-					     tos, vpninfo->dtls_tos_current);
-				if (setsockopt(vpninfo->dtls_fd, vpninfo->dtls_tos_proto,
-					       vpninfo->dtls_tos_optname, (void *)&tos, sizeof(tos)))
-					vpn_perror(vpninfo, _("UDP setsockopt"));
-				else
-					vpninfo->dtls_tos_current = tos;
-			}
-		}
+		if (vpninfo->dtls_tos_optname)
+			udp_tos_update(vpninfo, this);
 
 		/* One byte of header */
 		this->cstp.hdr[7] = AC_PKT_DATA;
diff --git a/esp.c b/esp.c
index 42c51c3e..b25a468a 100644
--- a/esp.c
+++ b/esp.c
@@ -326,6 +326,13 @@ int esp_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable)
 				}
 			}
 
+			/* XX: Must precede in-place encryption of the packet, because
+			   IP header fields (version and TOS) are garbled afterward.
+			   If TOS optname is set, we want to copy the TOS/TCLASS header
+			   to the outer UDP packet */
+			if (vpninfo->dtls_tos_optname)
+				udp_tos_update(vpninfo, this);
+
 			len = construct_esp_packet(vpninfo, this, 0);
 			if (len < 0) {
 				/* Should we disable ESP? */
diff --git a/openconnect-internal.h b/openconnect-internal.h
index 9d4f62b2..a75b2cb0 100644
--- a/openconnect-internal.h
+++ b/openconnect-internal.h
@@ -880,6 +880,7 @@ void destroy_eap_ttls(struct openconnect_info *vpninfo, void *sess);
 
 /* dtls.c */
 int dtls_setup(struct openconnect_info *vpninfo, int dtls_attempt_period);
+int udp_tos_update(struct openconnect_info *vpninfo, struct pkt *pkt);
 int dtls_mainloop(struct openconnect_info *vpninfo, int *timeout, int readable);
 void dtls_close(struct openconnect_info *vpninfo);
 void dtls_shutdown(struct openconnect_info *vpninfo);