reflect the received DPD packets

src/worker-vpn.c

@@ -1876,12 +1876,23 @@ static int connect_handler(worker_st * ws)
 	return -1;
 }
 
-static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interface of recv */
-		      uint8_t head, uint8_t * buf, size_t buf_size, time_t now)
+static int parse_data(struct worker_st *ws, uint8_t *buf, size_t buf_size,
+		      time_t now, unsigned is_dtls)
 {
 	int ret, e;
-	uint8_t *plain = buf;
-	ssize_t plain_size = buf_size;
+	uint8_t *plain;
+	ssize_t plain_size;
+	unsigned head;
+
+	if (is_dtls == 0) { /* CSTP */
+		plain = buf + 8;
+		plain_size = buf_size - 8;
+		head = buf[6];
+	} else {
+		plain = buf + 1;
+		plain_size = buf_size - 1;
+		head = buf[0];
+	}
 
 	switch (head) {
 	case AC_PKT_DPD_RESP:
@@ -1891,8 +1902,9 @@ static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interfac
 		oclog(ws, LOG_TRANSFER_DEBUG, "received keepalive");
 		break;
 	case AC_PKT_DPD_OUT:
-		if (ws->session == ts) {
-			ret = cstp_send(ws, "STF\x01\x00\x00\x04\x00", 8);
+		if (is_dtls == 0) {
+			buf[6] = AC_PKT_DPD_RESP;
+			ret = cstp_send(ws, buf, buf_size);
 
 			oclog(ws, LOG_TRANSFER_DEBUG,
 			      "received TLS DPD; sent response (%d bytes)",
@@ -1904,12 +1916,12 @@ static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interfac
 			}
 		} else {
 			/* Use DPD for MTU discovery in DTLS */
-			ws->buffer[0] = AC_PKT_DPD_RESP;
+			buf[0] = AC_PKT_DPD_RESP;
 
-			ret = dtls_send(ws, ws->buffer, 1);
+			ret = dtls_send(ws, buf, buf_size);
 			if (ret == GNUTLS_E_LARGE_PACKET) {
 				mtu_not_ok(ws);
-				ret = dtls_send(ws, ws->buffer, 1);
+				ret = dtls_send(ws, buf, 1);
 			}
 
 			oclog(ws, LOG_TRANSFER_DEBUG,
@@ -1930,22 +1942,22 @@ static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interfac
 		break;
 	case AC_PKT_COMPRESSED:
 		/* decompress */
-		if (ws->session == ts) { /* CSTP */
+		if (is_dtls == 0) { /* CSTP */
 			if (ws->cstp_selected_comp == NULL) {
 				oclog(ws, LOG_ERR, "received compression data but no compression was negotiated");
 				return -1;
 			}
 
-			plain_size = ws->cstp_selected_comp->decompress(ws->decomp, sizeof(ws->decomp), buf, buf_size);
-			oclog(ws, LOG_DEBUG, "decompressed %d to %d\n", (int)buf_size, (int)plain_size);
+			plain_size = ws->cstp_selected_comp->decompress(ws->decomp, sizeof(ws->decomp), plain, plain_size);
+			oclog(ws, LOG_DEBUG, "decompressed %d to %d\n", (int)buf_size-8, (int)plain_size);
 		} else { /* DTLS */
 			if (ws->dtls_selected_comp == NULL) {
 				oclog(ws, LOG_ERR, "received compression data but no compression was negotiated");
 				return -1;
 			}
 
-			plain_size = ws->dtls_selected_comp->decompress(ws->decomp, sizeof(ws->decomp), buf, buf_size);
-			oclog(ws, LOG_DEBUG, "decompressed %d to %d\n", (int)buf_size, (int)plain_size);
+			plain_size = ws->dtls_selected_comp->decompress(ws->decomp, sizeof(ws->decomp), plain, plain_size);
+			oclog(ws, LOG_DEBUG, "decompressed %d to %d\n", (int)buf_size-1, (int)plain_size);
 		}
 
 		if (plain_size <= 0) {
@@ -1956,7 +1968,7 @@ static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interfac
 		/* fall through */
 	case AC_PKT_DATA:
 		oclog(ws, LOG_TRANSFER_DEBUG, "writing %d byte(s) to TUN",
-		      (int)buf_size);
+		      (int)plain_size);
 		ret = tun_write(ws->tun_fd, plain, plain_size);
 		if (ret == -1) {
 			e = errno;
@@ -1964,13 +1976,13 @@ static int parse_data(struct worker_st *ws, gnutls_session_t ts,	/* the interfac
 			      strerror(e));
 			return -1;
 		}
-		ws->tun_bytes_in += buf_size;
+		ws->tun_bytes_in += plain_size;
 		ws->last_nc_msg = now;
 
 		break;
 	default:
-		oclog(ws, LOG_DEBUG, "received unknown packet %u",
-		      (unsigned)head);
+		oclog(ws, LOG_DEBUG, "received unknown packet %u/size: %u",
+		      (unsigned)head, (unsigned)buf_size);
 	}
 
 	return head;
@@ -1996,11 +2008,12 @@ static int parse_cstp_data(struct worker_st *ws,
 
 	pktlen = (buf[4] << 8) + buf[5];
 	if (buf_size != 8 + pktlen) {
-		oclog(ws, LOG_INFO, "unexpected CSTP length");
+		oclog(ws, LOG_INFO, "unexpected CSTP length (have %u, should be %d)",
+		      (unsigned)pktlen, (unsigned)buf_size-8);
 		return -1;
 	}
 
-	ret = parse_data(ws, ws->session, buf[6], buf + 8, pktlen, now);
+	ret = parse_data(ws, buf, buf_size, now, 0);
 	/* whatever we received treat it as DPD response.
 	 * it indicates that the channel is alive */
 	ws->last_msg_tcp = now;
@@ -2021,8 +2034,7 @@ static int parse_dtls_data(struct worker_st *ws,
 	}
 
 	ret =
-	    parse_data(ws, ws->dtls_session, buf[0], buf + 1, buf_size - 1,
-		       now);
+	    parse_data(ws, buf, buf_size, now, 1);
 	ws->last_msg_udp = now;
 	return ret;
 }