corrected DTLS packet handling.

src/config.c

@@ -204,6 +204,9 @@ int cmd_parser (int argc, char **argv, struct cfg_st* config)
 
 	if (HAVE_OPT(TLS_DEBUG))
 		config->tls_debug = 1;
+
+	if (HAVE_OPT(DEBUG))
+		config->debug = 1;
 	
 	if (HAVE_OPT(CONFIG)) {
 		cfg_file = OPT_ARG(CONFIG);

src/log.c

@@ -72,6 +72,9 @@ void __attribute__ ((format(printf, 3, 4)))
 	char ipbuf[128];
 	const char* ip;
 	va_list args;
+	
+	if (priority == LOG_DEBUG && ws->config->debug == 0)
+		return;
 
 	ip = human_addr((void*)&ws->remote_addr, ws->remote_addr_len,
 			    ipbuf, sizeof(ipbuf));

src/ocserv-args.c

@@ -2,7 +2,7 @@
  *  
  *  DO NOT EDIT THIS FILE   (ocserv-args.c)
  *  
- *  It has been AutoGen-ed  February  5, 2013 at 09:21:04 PM by AutoGen 5.16
+ *  It has been AutoGen-ed  February  5, 2013 at 10:37:10 PM by AutoGen 5.16
  *  From the definitions    ocserv-args.def
  *  and the template file   options
  *
@@ -65,7 +65,7 @@ extern FILE * option_usage_fp;
 /*
  *  ocserv option static const strings
  */
-static char const ocserv_opt_strs[1282] =
+static char const ocserv_opt_strs[1340] =
 /*     0 */ "ocserv\n"
             "Copyright (C) 2013 Nikos Mavrogiannopoulos, all rights reserved.\n"
             "This is free software. It is licensed for use, modification and\n"
@@ -86,21 +86,24 @@ static char const ocserv_opt_strs[1282] =
 /*   854 */ "Enable verbose TLS debugging information.\0"
 /*   896 */ "TLS_DEBUG\0"
 /*   906 */ "tls-debug\0"
-/*   916 */ "Configuration file for the server.\0"
-/*   951 */ "CONFIG\0"
-/*   958 */ "config\0"
-/*   965 */ "Display extended usage information and exit\0"
-/*  1009 */ "help\0"
-/*  1014 */ "Extended usage information passed thru pager\0"
-/*  1059 */ "more-help\0"
-/*  1069 */ "OCSERV\0"
-/*  1076 */ "ocserv - OpenConnect server\n"
+/*   916 */ "Enable verbose network debugging information.\0"
+/*   962 */ "DEBUG\0"
+/*   968 */ "debug\0"
+/*   974 */ "Configuration file for the server.\0"
+/*  1009 */ "CONFIG\0"
+/*  1016 */ "config\0"
+/*  1023 */ "Display extended usage information and exit\0"
+/*  1067 */ "help\0"
+/*  1072 */ "Extended usage information passed thru pager\0"
+/*  1117 */ "more-help\0"
+/*  1127 */ "OCSERV\0"
+/*  1134 */ "ocserv - OpenConnect server\n"
             "USAGE:  %s [ -<flag> [<val>] | --<name>[{=| }<val>] ]...\n\0"
-/*  1162 */ "nmav@gnutls.org\0"
-/*  1178 */ "\n\n\0"
-/*  1181 */ "\n"
+/*  1220 */ "nmav@gnutls.org\0"
+/*  1236 */ "\n\n\0"
+/*  1239 */ "\n"
             "OpenConnect VPN server.\n\0"
-/*  1207 */ "Usage: ocserv [options] -c [config]\n"
+/*  1265 */ "Usage: ocserv [options] -c [config]\n"
             "ocserv --help for usage instructions.\n";
 
 /*
@@ -119,23 +122,31 @@ static char const ocserv_opt_strs[1282] =
 #define TLS_DEBUG_name      (ocserv_opt_strs+906)
 #define TLS_DEBUG_FLAGS     (OPTST_DISABLED)
 
+/*
+ *  debug option description:
+ */
+#define DEBUG_DESC      (ocserv_opt_strs+916)
+#define DEBUG_NAME      (ocserv_opt_strs+962)
+#define DEBUG_name      (ocserv_opt_strs+968)
+#define DEBUG_FLAGS     (OPTST_DISABLED)
+
 /*
  *  config option description:
  */
-#define CONFIG_DESC      (ocserv_opt_strs+916)
-#define CONFIG_NAME      (ocserv_opt_strs+951)
-#define CONFIG_name      (ocserv_opt_strs+958)
+#define CONFIG_DESC      (ocserv_opt_strs+974)
+#define CONFIG_NAME      (ocserv_opt_strs+1009)
+#define CONFIG_name      (ocserv_opt_strs+1016)
 #define CONFIG_FLAGS     (OPTST_DISABLED \
         | OPTST_SET_ARGTYPE(OPARG_TYPE_FILE))
 
 /*
  *  Help/More_Help option descriptions:
  */
-#define HELP_DESC       (ocserv_opt_strs+965)
-#define HELP_name       (ocserv_opt_strs+1009)
+#define HELP_DESC       (ocserv_opt_strs+1023)
+#define HELP_name       (ocserv_opt_strs+1067)
 #ifdef HAVE_WORKING_FORK
-#define MORE_HELP_DESC  (ocserv_opt_strs+1014)
-#define MORE_HELP_name  (ocserv_opt_strs+1059)
+#define MORE_HELP_DESC  (ocserv_opt_strs+1072)
+#define MORE_HELP_name  (ocserv_opt_strs+1117)
 #define MORE_HELP_FLAGS (OPTST_IMM | OPTST_NO_INIT)
 #else
 #define MORE_HELP_DESC  NULL
@@ -184,8 +195,20 @@ static tOptDesc optDesc[OPTION_CT] = {
      /* desc, NAME, name */ TLS_DEBUG_DESC, TLS_DEBUG_NAME, TLS_DEBUG_name,
      /* disablement strs */ NULL, NULL },
 
-  {  /* entry idx, value */ 2, VALUE_OPT_CONFIG,
-     /* equiv idx, value */ 2, VALUE_OPT_CONFIG,
+  {  /* entry idx, value */ 2, VALUE_OPT_DEBUG,
+     /* equiv idx, value */ 2, VALUE_OPT_DEBUG,
+     /* equivalenced to  */ NO_EQUIVALENT,
+     /* min, max, act ct */ 0, 1, 0,
+     /* opt state flags  */ DEBUG_FLAGS, 0,
+     /* last opt argumnt */ { NULL }, /* --debug */
+     /* arg list/cookie  */ NULL,
+     /* must/cannot opts */ NULL, NULL,
+     /* option proc      */ NULL,
+     /* desc, NAME, name */ DEBUG_DESC, DEBUG_NAME, DEBUG_name,
+     /* disablement strs */ NULL, NULL },
+
+  {  /* entry idx, value */ 3, VALUE_OPT_CONFIG,
+     /* equiv idx, value */ 3, VALUE_OPT_CONFIG,
      /* equivalenced to  */ NO_EQUIVALENT,
      /* min, max, act ct */ 0, 1, 0,
      /* opt state flags  */ CONFIG_FLAGS, 0,
@@ -226,13 +249,13 @@ static tOptDesc optDesc[OPTION_CT] = {
  *
  *  Define the ocserv Option Environment
  */
-#define zPROGNAME       (ocserv_opt_strs+1069)
-#define zUsageTitle     (ocserv_opt_strs+1076)
+#define zPROGNAME       (ocserv_opt_strs+1127)
+#define zUsageTitle     (ocserv_opt_strs+1134)
 #define zRcName         NULL
 #define apzHomeList     NULL
-#define zBugsAddr       (ocserv_opt_strs+1162)
-#define zExplain        (ocserv_opt_strs+1178)
-#define zDetail         (ocserv_opt_strs+1181)
+#define zBugsAddr       (ocserv_opt_strs+1220)
+#define zExplain        (ocserv_opt_strs+1236)
+#define zDetail         (ocserv_opt_strs+1239)
 #define zFullVersion    (NULL)
 /* extracted from optcode.tlib near line 350 */
 
@@ -247,7 +270,7 @@ static tOptDesc optDesc[OPTION_CT] = {
 
 #define ocserv_full_usage (NULL)
 
-#define ocserv_short_usage (ocserv_opt_strs+1207)
+#define ocserv_short_usage (ocserv_opt_strs+1265)
 
 #endif /* not defined __doxygen__ */
 
@@ -353,7 +376,7 @@ tOptions ocservOptions = {
       NO_EQUIVALENT, /* '-#' option index */
       NO_EQUIVALENT /* index of default opt */
     },
-    5 /* full option count */, 3 /* user option count */,
+    6 /* full option count */, 4 /* user option count */,
     ocserv_full_usage, ocserv_short_usage,
     NULL, NULL,
     PKGDATADIR, ocserv_packager_info

src/ocserv-args.def

@@ -33,6 +33,13 @@ flag = {
     doc      = "";
 };
 
+flag = {
+    name      = debug;
+    value     = d;
+    descrip   = "Enable verbose network debugging information.";
+    doc      = "";
+};
+
 flag = {
     name      = config;
     value     = c;

src/ocserv-args.h

@@ -2,7 +2,7 @@
  *  
  *  DO NOT EDIT THIS FILE   (ocserv-args.h)
  *  
- *  It has been AutoGen-ed  February  5, 2013 at 09:21:04 PM by AutoGen 5.16
+ *  It has been AutoGen-ed  February  5, 2013 at 10:37:10 PM by AutoGen 5.16
  *  From the definitions    ocserv-args.def
  *  and the template file   options
  *
@@ -68,12 +68,13 @@
 typedef enum {
     INDEX_OPT_FOREGROUND  =  0,
     INDEX_OPT_TLS_DEBUG   =  1,
-    INDEX_OPT_CONFIG      =  2,
-    INDEX_OPT_HELP        =  3,
-    INDEX_OPT_MORE_HELP   =  4
+    INDEX_OPT_DEBUG       =  2,
+    INDEX_OPT_CONFIG      =  3,
+    INDEX_OPT_HELP        =  4,
+    INDEX_OPT_MORE_HELP   =  5
 } teOptIndex;
 
-#define OPTION_CT    5
+#define OPTION_CT    6
 
 /*
  *  Interface defines for all options.  Replace "n" with the UPPER_CASED
@@ -111,6 +112,7 @@ typedef enum {
  */
 #define VALUE_OPT_FOREGROUND     'f'
 #define VALUE_OPT_TLS_DEBUG      1
+#define VALUE_OPT_DEBUG          'd'
 #define VALUE_OPT_CONFIG         'c'
 #define VALUE_OPT_HELP          'h'
 #define VALUE_OPT_MORE_HELP     '!'

src/vpn.h

@@ -60,6 +60,7 @@ struct cfg_st {
 	const char *cookie_db;
 	unsigned foreground;
 	unsigned tls_debug;
+	unsigned debug;
 	unsigned max_clients;
 
 	const char *connect_script;

src/worker-auth.c

@@ -268,6 +268,8 @@ int auth_cookie(worker_st *ws, void* cookie, size_t cookie_size)
 int ret;
 struct cmd_auth_cookie_req_st areq;
 
+	memset(&areq, 0, sizeof(areq));
+
 	if (cookie_size != sizeof(areq.cookie))
 		return -1;
 

src/worker-vpn.c

@@ -58,7 +58,9 @@
 #define MAX_HTTP_REQUESTS 8
 
 static int handle_worker_commands(struct worker_st *ws);
-static int parse_cstp_data(struct worker_st* ws, gnutls_session_t, uint8_t* buf, size_t buf_size);
+static int parse_cstp_data(struct worker_st* ws, uint8_t* buf, size_t buf_size);
+static int parse_dtls_data(struct worker_st* ws, 
+				uint8_t* buf, size_t buf_size);
 
 static void handle_alarm(int signo)
 {
@@ -120,8 +122,6 @@ int url_cb(http_parser* parser, const char *at, size_t length)
 	memcpy(req->url, at, length);
 	req->url[length] = 0;
 
-	//fprintf(stderr, "request %s %s\n", http_method_str(parser->method), req->url);
-
 	return 0;
 }
 
@@ -189,6 +189,11 @@ size_t nlen;
 
 				nlen = sizeof(req->cookie);
 				gnutls_hex2bin(p, length, req->cookie, &nlen);
+
+				if (nlen < COOKIE_SIZE) {
+					req->cookie_set = 0;
+					return 0;
+				}
 				req->cookie_set = 1;
 				break;
 		}
@@ -240,6 +245,7 @@ uint8_t buffer[512];
 ssize_t buffer_size;
 gnutls_datum_t master = { ws->master_secret, sizeof(ws->master_secret) };
 gnutls_datum_t sid = { ws->session_id, sizeof(ws->session_id) };
+unsigned port;
 
 	/* first receive from the correct client and connect socket */
 	cli_addr_size = sizeof(cli_addr);
@@ -248,7 +254,14 @@ gnutls_datum_t sid = { ws->session_id, sizeof(ws->session_id) };
 		oclog(ws, LOG_ERR, "Error receiving in UDP socket");
 		return -1;
 	}
-		
+	
+	if (cli_addr_size == sizeof(struct sockaddr_in6)) {
+		port = SA_IN6_PORT(&cli_addr);
+	} else {
+		port = SA_IN_PORT(&cli_addr);
+	}
+	port = ntohs(port);
+	
 	buffer_size = ret;
 
 	if ( (ws->remote_addr_len == sizeof(struct sockaddr_in) && memcmp(SA_IN_P(&cli_addr), 
@@ -257,12 +270,15 @@ gnutls_datum_t sid = { ws->session_id, sizeof(ws->session_id) };
 		SA_IN6_P(&ws->remote_addr), sizeof(struct in6_addr)) == 0)) {
 
 		/* connect to host */
+#if 1
+		oclog(ws, LOG_ERR, "Connecting to UDP port %u", port);
 		ret = connect(ws->udp_fd, (void*)&cli_addr, cli_addr_size);
 		if (ret < 0) {
 			e = errno;
 			oclog(ws, LOG_ERR, "Error connecting: %s", strerror(e));
 			return -1;
 		}
+#endif
 	} else {
 		/* received packet from unknown host */
 
@@ -450,6 +466,33 @@ finish:
 	tls_close(session);
 }
 
+static int set_tun_mtu(struct worker_st* ws, unsigned mtu)
+{
+int fd, ret, e;
+struct ifreq ifr;
+
+	fd = socket(AF_INET, SOCK_STREAM, 0);
+	if (fd == -1)
+		return -1;
+
+	memset(&ifr, 0, sizeof(ifr));
+	snprintf(ifr.ifr_name, IFNAMSIZ, "%s", ws->tun_name);
+	ifr.ifr_mtu = mtu;
+	
+	ret = ioctl(fd, SIOCSIFMTU, &ifr);
+	if (ret != 0) {
+		e = errno;
+		oclog(ws, LOG_DEBUG, "ioctl SIOCSIFMTU error: %s", strerror(e));
+		ret = -1;
+		goto fail;
+	}
+	
+	ret = 0;
+fail:
+	close(fd);
+	return ret;
+}
+
 /* if local is non zero it returns the local, otherwise the remote */
 static int get_ip(struct worker_st* ws, int fd, int family, unsigned int local,
 	         struct vpn_st* vinfo, char** buffer, size_t* buffer_size)
@@ -707,6 +750,54 @@ const uint8_t * p = buf;
 	return len;
 }
 
+/* sets the provided value of mtu as bad,
+ * and returns an estimation of good.
+ *
+ * Returns -1 on failure.
+ */
+static
+int mtu_not_ok(worker_st* ws, unsigned *mtu)
+{
+	ws->last_bad_mtu = *mtu;
+	
+	ws->last_good_mtu = (*mtu)/2;
+	
+	if (ws->last_good_mtu < 128)
+		return -1;
+
+	*mtu = ws->last_good_mtu;
+	gnutls_dtls_set_data_mtu (ws->dtls_session, *mtu);
+	set_tun_mtu(ws, *mtu);
+
+	return 0;
+}
+
+static void mtu_set(worker_st *ws, unsigned max_mtu)
+{
+	if (ws->last_good_mtu == 0)
+		ws->last_good_mtu = max_mtu;
+	if (ws->last_bad_mtu == 0)
+		ws->last_bad_mtu = max_mtu;
+}
+
+static
+void mtu_ok(worker_st* ws, unsigned sent, unsigned *mtu)
+{
+int c;
+
+	if (sent < *mtu || ws->last_bad_mtu == (*mtu)+1)
+		return;
+		
+
+	c = (ws->last_good_mtu + ws->last_bad_mtu)/2;
+
+	*mtu = c;
+	gnutls_dtls_set_data_mtu (ws->dtls_session, c);
+	set_tun_mtu(ws, c);
+
+	return;
+}
+
 static int connect_handler(worker_st *ws)
 {
 int ret;
@@ -717,12 +808,11 @@ int max;
 unsigned i;
 struct vpn_st vinfo;
 uint8_t buffer[16*1024];
-unsigned int buffer_size;
+unsigned int buffer_size, tls_retry;
 char *p;
 unsigned tls_pending, dtls_pending = 0;
 time_t udp_recv_time = 0;
-unsigned mtu_overhead, effective_mtu = 0;
-gnutls_session_t ts;
+unsigned mtu_overhead, dtls_mtu = 0;
 
 	if (req->cookie_set == 0) {
 		oclog(ws, LOG_INFO, "Connect request without authentication");
@@ -833,9 +923,9 @@ gnutls_session_t ts;
 			mtu_overhead = 20+8;
 		else
 			mtu_overhead = 40+8;
-		effective_mtu = vinfo.mtu - mtu_overhead;
+		dtls_mtu = vinfo.mtu - mtu_overhead;
 
-		tls_printf(ws->session, "X-DTLS-MTU: %u\r\n", effective_mtu);
+		tls_printf(ws->session, "X-DTLS-MTU: %u\r\n", dtls_mtu);
 	}
 
 	tls_puts(ws->session, "X-CSTP-Banner: Welcome\r\n");
@@ -866,15 +956,13 @@ gnutls_session_t ts;
 		}
 		
 		if (FD_ISSET(ws->tun_fd, &rfds)) {
-
+				
 			if (ws->udp_state == UP_ACTIVE) {
-				l = effective_mtu;
-				ts = ws->dtls_session;
+				l = dtls_mtu;
 			} else {
 				l = sizeof(buffer);
-				ts = ws->session;
 			}
-				
+
 			l = read(ws->tun_fd, buffer + 8, l - 8);
 			if (l < 0) {
 				e = errno;
@@ -891,31 +979,48 @@ gnutls_session_t ts;
 				exit(1);
 			}
 
-			buffer[0] = 'S';
-			buffer[1] = 'T';
-			buffer[2] = 'F';
-			buffer[3] = 1;
-			buffer[4] = l >> 8;
-			buffer[5] = l & 0xff;
-			buffer[6] = 0;
-			buffer[7] = 0;
+			tls_retry = 0;
+			oclog(ws, LOG_DEBUG, "Sending %d\n", l);
+			if (ws->udp_state == UP_ACTIVE) {
+				buffer[7] = AC_PKT_DATA;
 
-			ret = tls_send(ts, buffer, l + 8);
-			GNUTLS_FATAL_ERR(ret);
+				ret = tls_send(ws->dtls_session, buffer + 7, l + 1);
+				GNUTLS_FATAL_ERR(ret);
 
-			if (ret == GNUTLS_E_LARGE_PACKET) {
-				/* XXX: we have to do something better than that.
-				 * adjust mtu */
-				if (effective_mtu > 128)
-					effective_mtu -= 32;
+				if (ret == GNUTLS_E_LARGE_PACKET) {
+					ret = mtu_not_ok(ws, &dtls_mtu);
+					if (ret < 0) {
+						oclog(ws, LOG_INFO, "Could not calculate an MTU. Disabling DTLS.");
+						ws->udp_state = UP_DISABLED;
+					}
+
+					oclog(ws, LOG_DEBUG, "Retrying (TLS) %d\n", l);
+					tls_retry = 1;
+				} else if (ret > 0) {
+					 mtu_ok(ws, ret, &dtls_mtu);
+				}
+			}
+
+			if (ws->udp_state != UP_ACTIVE || tls_retry != 0) {
+				buffer[0] = 'S';
+				buffer[1] = 'T';
+				buffer[2] = 'F';
+				buffer[3] = 1;
+				buffer[4] = l >> 8;
+				buffer[5] = l & 0xff;
+				buffer[6] = AC_PKT_DATA;
+				buffer[7] = 0;
 
 				ret = tls_send(ws->session, buffer, l + 8);
 				GNUTLS_FATAL_ERR(ret);
 			}
+
 		}
 
 		if (FD_ISSET(ws->conn_fd, &rfds) || tls_pending != 0) {
 			ret = tls_recv(ws->session, buffer, sizeof(buffer));
+			oclog(ws, LOG_DEBUG, "Received %d bytes (TLS)\n", ret);
+
 			GNUTLS_FATAL_ERR(ret);
 
 			if (ret == 0) { /* disconnect */
@@ -925,7 +1030,7 @@ gnutls_session_t ts;
 
 			l = ret;
 
-			ret = parse_cstp_data(ws, ws->session, buffer, l);
+			ret = parse_cstp_data(ws, buffer, l);
 			if (ret < 0) {
 				oclog(ws, LOG_INFO, "Error parsing CSTP data");
 				exit(1);
@@ -944,6 +1049,8 @@ gnutls_session_t ts;
 				case UP_ACTIVE:
 				case UP_INACTIVE:
 					ret = tls_recv(ws->dtls_session, buffer, sizeof(buffer));
+					oclog(ws, LOG_DEBUG, "Received %d bytes (DTLS)\n", ret);
+
 					GNUTLS_FATAL_ERR(ret);
 
 
@@ -955,7 +1062,7 @@ gnutls_session_t ts;
 
 					ws->udp_state = UP_ACTIVE;
 
-					ret = parse_cstp_data(ws, ws->dtls_session, buffer, l);
+					ret = parse_dtls_data(ws, buffer, l);
 					if (ret < 0) {
 						oclog(ws, LOG_INFO, "Error parsing CSTP data");
 						exit(1);
@@ -968,9 +1075,12 @@ gnutls_session_t ts;
 					if (ret < 0)
 						exit(1);
 					
-					gnutls_dtls_set_mtu (ws->dtls_session, effective_mtu);
+					gnutls_dtls_set_mtu (ws->dtls_session, dtls_mtu);
+					mtu_set(ws, dtls_mtu);
+
 					break;
 				case UP_HANDSHAKE:
+hsk_restart:
 					ret = gnutls_handshake(ws->dtls_session);
 					if (ret < 0 && gnutls_error_is_fatal(ret) != 0) {
 						oclog(ws, LOG_ERR, "Error in DTLS handshake: %s\n", gnutls_strerror(ret));
@@ -980,15 +1090,20 @@ gnutls_session_t ts;
 
 					if (ret == GNUTLS_E_LARGE_PACKET) {
 						/* adjust mtu */
-						if (effective_mtu > 256)
-							effective_mtu -= 128;
+						ret = mtu_not_ok(ws, &dtls_mtu);
+						if (dtls_mtu < 0) {
+							oclog(ws, LOG_DEBUG, "DTLS handshake failed. MTU error\n");
+						} else {
+							goto hsk_restart;
+						}
 
 					}
 
 					if (ret == 0) {
 						ws->udp_state = UP_ACTIVE;
-						effective_mtu = gnutls_dtls_get_data_mtu(ws->dtls_session);
-						oclog(ws, LOG_DEBUG, "DTLS handshake complete (MTU: %u)\n", effective_mtu);
+						dtls_mtu = gnutls_dtls_get_data_mtu(ws->dtls_session);
+						mtu_set(ws, dtls_mtu);
+						oclog(ws, LOG_DEBUG, "DTLS handshake completed (MTU: %u)\n", dtls_mtu);
 					}
 					
 					break;
@@ -1057,11 +1172,49 @@ int handle_worker_commands(struct worker_st *ws)
 	return 0;
 }
 
+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)
+{
+int ret, e;
+
+	switch (head) {
+		case AC_PKT_DPD_RESP:
+		case AC_PKT_KEEPALIVE:
+			break;
+
+		case AC_PKT_DPD_OUT:
+			oclog(ws, LOG_DEBUG, "Sending STF8\n");
+			ret =
+			    tls_send(ts, "STF\x01\x00\x00\x04\x00", 8);
+			if (ret < 0) {
+				oclog(ws, LOG_ERR, "Could not send TLS data: %s", gnutls_strerror(ret));
+				return -1;
+			}
+			break;
+		case AC_PKT_DISCONN:
+			oclog(ws, LOG_INFO, "Received BYE packet\n");
+			break;
+		case AC_PKT_DATA:
+			oclog(ws, LOG_DEBUG, "Writing %d bytes to TUN\n", (int)buf_size);
+			ret = tun_write(ws->tun_fd, buf, buf_size);
+			if (ret == -1) {
+				e = errno;
+				oclog(ws, LOG_ERR, "Could not write data to tun: %s", strerror(e));
+				return -1;
+			}
+
+			break;
+	}
+	
+	return head;
+}
+
 static int parse_cstp_data(struct worker_st* ws, 
-				gnutls_session_t ts, /* the interface of recv */
 				uint8_t* buf, size_t buf_size)
 {
-int pktlen, ret, e;
+int pktlen;
 
 	if (buf_size < 8) {
 		oclog(ws, LOG_INFO, "Can't read CSTP header (only %d bytes are available)\n", (int)buf_size);
@@ -1080,32 +1233,16 @@ int pktlen, ret, e;
 		return -1;
 	}
 
-	switch (buf[6]) {
-		case AC_PKT_DPD_RESP:
-		case AC_PKT_KEEPALIVE:
-			break;
-
-		case AC_PKT_DPD_OUT:
-			ret =
-			    tls_send(ts, "STF\x01\x00\x00\x04\x00", 8);
-			if (ret < 0) {
-				oclog(ws, LOG_ERR, "Could not send TLS data: %s", gnutls_strerror(ret));
-				return -1;
-			}
-			break;
-		case AC_PKT_DISCONN:
-			oclog(ws, LOG_INFO, "Received BYE packet\n");
-			break;
-		case AC_PKT_DATA:
-			ret = tun_write(ws->tun_fd, buf + 8, pktlen);
-			if (ret == -1) {
-				e = errno;
-				oclog(ws, LOG_ERR, "Could not write data to tun: %s", strerror(e));
-				return -1;
-			}
-
-			break;
-	}
-	
-	return buf[6];
+	return parse_data(ws, ws->session, buf[6], buf+8, pktlen);
+}
+
+static int parse_dtls_data(struct worker_st* ws, 
+				uint8_t* buf, size_t buf_size)
+{
+	if (buf_size < 1) {
+		oclog(ws, LOG_INFO, "Can't read DTLS header (only %d bytes are available)\n", (int)buf_size);
+		return -1;
+	}
+
+	return parse_data(ws, ws->dtls_session, buf[0], buf+1, buf_size-1);
 }