# HG changeset patch
# User Sebastien Decugis <sdecugis@freediameter.net>
# Date 1371541871 -28800
# Node ID 8802d3fa5876d64b5d5959419cb8d2508f5609cf
# Parent  876cb3e4d738363ac5586d88b3a466af7c210f55
Got successful handshake over DTLS with this version

diff -r 876cb3e4d738 -r 8802d3fa5876 freeDiameterd/main.c
--- a/freeDiameterd/main.c	Tue Jun 18 12:46:14 2013 +0800
+++ b/freeDiameterd/main.c	Tue Jun 18 15:51:11 2013 +0800
@@ -51,7 +51,10 @@
 
 /* gnutls debug */
 static void fd_gnutls_debug(int level, const char * str) {
-	fd_log_debug(" [gnutls:%d] %s", level, str);
+	int len = strlen(str);
+	if (str[len-1] == '\n')
+		len--;
+	fd_log_debug(" [gnutls:%d] %.*s", level, len, str);
 }
 
 
diff -r 876cb3e4d738 -r 8802d3fa5876 include/freeDiameter/libfdproto.h
--- a/include/freeDiameter/libfdproto.h	Tue Jun 18 12:46:14 2013 +0800
+++ b/include/freeDiameter/libfdproto.h	Tue Jun 18 15:51:11 2013 +0800
@@ -129,6 +129,7 @@
 /*                          DEBUG                             */
 /*============================================================*/
 
+#define LOGS_VALIGN
 
 /*
  * FUNCTION:	fd_log
@@ -296,7 +297,11 @@
 
 /* In DEBUG mode, we add meta-information along each trace. This makes multi-threading problems easier to debug. */
 #ifdef DEBUG
-# define STD_TRACE_FMT_STRING "pid:%s in %s@%s:%d: "
+# ifdef LOGS_VALIGN
+#  define STD_TRACE_FMT_STRING "pid:%-25.25s in %25.25s@%-15.15s:%-4d: "
+# else /* LOGS_VALIGN */
+#  define STD_TRACE_FMT_STRING "pid:%s in %s@%s:%d: "
+# endif /* LOGS_VALIGN */
 # define STD_TRACE_FMT_ARGS   , ((char *)pthread_getspecific(fd_log_thname) ?: "unnamed"), __PRETTY_FUNCTION__, __STRIPPED_FILE__, __LINE__
 #else /* DEBUG */
 # define STD_TRACE_FMT_STRING ""
diff -r 876cb3e4d738 -r 8802d3fa5876 libfdcore/p_cnx.c
--- a/libfdcore/p_cnx.c	Tue Jun 18 12:46:14 2013 +0800
+++ b/libfdcore/p_cnx.c	Tue Jun 18 15:51:11 2013 +0800
@@ -291,7 +291,7 @@
 	/* Handshake if needed (secure port) */
 	if (nc->dotls) {
 		CHECK_FCT_DO( fd_cnx_handshake(cnx, GNUTLS_CLIENT, 
-						ALGO_HANDSHAKE_3436,
+						(peer->p_hdr.info.config.pic_flags.sctpsec == PI_SCTPSEC_3436) ? ALGO_HANDSHAKE_3436 : ALGO_HANDSHAKE_DEFAULT,
 						peer->p_hdr.info.config.pic_priority, NULL),
 			{
 				/* Handshake failed ...  */
diff -r 876cb3e4d738 -r 8802d3fa5876 libfdcore/sctp_dtls.c
--- a/libfdcore/sctp_dtls.c	Tue Jun 18 12:46:14 2013 +0800
+++ b/libfdcore/sctp_dtls.c	Tue Jun 18 15:51:11 2013 +0800
@@ -143,7 +143,7 @@
 	switch (event) {
 		case FDEVP_CNX_MSG_RECV:
 			got_data = 1;
-			LOG_A("Received DTLS data, type %02hhx, Seq %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx, Stream %hu",
+			LOG_A("Received DTLS data, type %hhd, Seq %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx, Stream %hu",
 				(*buf)[0],
 				(*buf)[3],(*buf)[4],(*buf)[5],(*buf)[6], (*buf)[7],(*buf)[8],(*buf)[9],(*buf)[10],
 				*strid);
@@ -166,6 +166,63 @@
 	return got_data;
 }
 
+/* Count the number of records received in a chunk (including partial) and increment the nextseq field accordingly */
+static void update_nextseq_from_records(struct cnxctx * conn, uint8_t * buf, size_t len)
+{
+	size_t offset = 0;
+	uint16_t next_record_len;
+	int i;
+	
+	
+	while (offset + 13 <= len) {
+		next_record_len = (buf[offset+11] << 8) + buf[offset+12];
+		LOG_A("update_nextseq_from_records off:%zd Type %hhd, Ver:%02hhx.%02hhx, Seq:%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx",
+			offset,
+			buf[offset], buf[offset+1], buf[offset+2], 
+			buf[offset+3],buf[offset+4],buf[offset+5],buf[offset+6],buf[offset+7],buf[offset+8],buf[offset+9],buf[offset+10]
+			);
+		
+		if (memcmp(buf + offset + DTLS_SEQ_OFFSET, conn->cc_sctp_dtls_data.nextseq, 8) != 0) {
+			/* The next record is not the one we expect in sequence. Is it a new epoch ? */
+			uint8_t newepoch[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+			if (conn->cc_sctp_dtls_data.nextseq[1] != 0xFF) {
+				newepoch[0] = conn->cc_sctp_dtls_data.nextseq[0]; newepoch[1] = conn->cc_sctp_dtls_data.nextseq[1] + 1;
+			} else if (conn->cc_sctp_dtls_data.nextseq[0] != 0xFF) {
+				newepoch[0] =conn->cc_sctp_dtls_data.nextseq[0] + 1; newepoch[1] = 0;
+			} else {
+				LOG_F("Epoch field wrapped, can this happen ???");
+				ASSERT(0); TODO("FFS");
+			}
+			
+			if (memcmp(buf + offset + DTLS_SEQ_OFFSET, newepoch, 8) == 0) {
+				/* Yes, this is a new epoch record, store this as next seq and continue */
+				memcpy(conn->cc_sctp_dtls_data.nextseq, newepoch, 8);
+			} else {
+				LOG_E("buf seq: %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", buf[offset +3], buf[offset +4], buf[offset +5], buf[offset +6], buf[offset +7], buf[offset +8], buf[offset +9], buf[offset +10]);
+				LOG_E("nextseq: %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", conn->cc_sctp_dtls_data.nextseq[0], conn->cc_sctp_dtls_data.nextseq[1], conn->cc_sctp_dtls_data.nextseq[2], conn->cc_sctp_dtls_data.nextseq[3], conn->cc_sctp_dtls_data.nextseq[4], conn->cc_sctp_dtls_data.nextseq[5], conn->cc_sctp_dtls_data.nextseq[6], conn->cc_sctp_dtls_data.nextseq[7]);
+				TODO("This should not be happening...");
+				ASSERT(0);
+			}
+		}
+		
+		/* increment seq number */
+		for (i = 7; i>=3; i--) {
+			if (conn->cc_sctp_dtls_data.nextseq[i] == 0xFF) {
+				conn->cc_sctp_dtls_data.nextseq[i] = 0;
+			} else {
+				conn->cc_sctp_dtls_data.nextseq[i] ++;
+				break;
+			}
+		}
+		if (i==2) {
+			LOG_F("Sequence_number field wrapped, can this happen ???");
+			ASSERT(0); TODO("FFS");
+		}
+		
+		offset += (size_t)next_record_len + 13;
+	}
+}
+
 /***************************************************************************************************/
 /* Helper functions to reorder the received chunks by sequence number                              */
 /***************************************************************************************************/
@@ -230,7 +287,7 @@
 static size_t chunk_retrieve(struct cnxctx * conn, void * upperbuf, size_t upperlen, int probeonly) 
 {
 	struct chunk * next;
-	int cmp,i;
+	int cmp;
 	size_t ret = 0;
 redo:	
 	if (FD_IS_LIST_EMPTY(&conn->cc_sctp_dtls_data.chunks)) {
@@ -263,7 +320,7 @@
 	cmp = memcmp(next->seq, conn->cc_sctp_dtls_data.nextseq, 8);
 	if (cmp < 0) {
 		cmp = memcmp(next->seq, conn->cc_sctp_dtls_data.validseq, 8);
-		if (cmp <= 0) {
+		if (cmp < 0) {
 			/* This is old stuff or invalid stuff, discard */
 			LOG_E("Unqueued DTLS packet with old sequence number, discarding.");
 			free(next->buffer);
@@ -271,9 +328,10 @@
 			free(next);
 			goto redo;
 		}
-		/* If the first chunk in our list has a smaller seq number than what we already delivered, we pass it above (to prevent DoS) */
-		LOG_A("Unqueueing chunk with seq number %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", 
-			next->seq[0],next->seq[1],next->seq[2],next->seq[3],next->seq[4],next->seq[5],next->seq[6],next->seq[7]);
+		/* If the first chunk in our list has a smaller seq number than what we already delivered, we pass it above (to prevent possible DoS by sending forged sequence numbers) */
+		if (probeonly)
+			return 1;
+		
 		ret = next->len;
 		if (upperlen < ret) {
 			ret = upperlen;
@@ -285,6 +343,8 @@
 			fd_list_unlink(&next->chain);
 			free(next);
 		}
+		LOG_A("Unqueueing (old) chunk with seq number %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", 
+			next->seq[0],next->seq[1],next->seq[2],next->seq[3],next->seq[4],next->seq[5],next->seq[6],next->seq[7]);
 		return ret;
 	}
 	if (cmp > 0) {
@@ -301,10 +361,12 @@
 		
 		if (memcmp(newepoch, next->seq, 8) == 0) {
 			/* Bingo, this is the first message of the new epoch. We update our nextseq accordingly */
+			if (probeonly)
+				return 1;
 			memcpy(conn->cc_sctp_dtls_data.nextseq, newepoch, 8);
-			conn->cc_sctp_dtls_data.nextseq[7] = 1;
+			update_nextseq_from_records(conn, next->buffer, next->len);
 			
-			LOG_A("Unqueueing chunk with seq number %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", 
+			LOG_A("Unqueueing chunk with seq number %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx (epoch change)", 
 				next->seq[0],next->seq[1],next->seq[2],next->seq[3],next->seq[4],next->seq[5],next->seq[6],next->seq[7]);
 			ret = next->len;
 			if (upperlen < ret) {
@@ -325,23 +387,15 @@
 	}
 	
 	/* next is the next chunk expected on this connection */
+	if (probeonly)
+		return 1;
 	
-	/* We increment the next seq */
-	for (i = 7; i>=3; i--) {
-		if (conn->cc_sctp_dtls_data.nextseq[i] == 0xFF) {
-			conn->cc_sctp_dtls_data.nextseq[i] = 0;
-		} else {
-			conn->cc_sctp_dtls_data.nextseq[i] ++;
-			break;
-		}
-	}
-	if (i==2) {
-		LOG_F("Sequence_number field wrapped, can this happen ???");
-		ASSERT(0); TODO("FFS");
-	}
+	/* We increment the next seq by the number or records found in this chunk */
+	update_nextseq_from_records(conn, next->buffer, next->len);
 	
 	/* And we deliver this to upper layer */
-	LOG_A("Unqueueing chunk with seq number %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", 
+	LOG_A("Unqueueing chunk: Type %hhd, Ver:%02hhx.%02hhx, Seq:%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", 
+		next->buffer[0], next->buffer[1], next->buffer[2], 
 		next->seq[0],next->seq[1],next->seq[2],next->seq[3],next->seq[4],next->seq[5],next->seq[6],next->seq[7]);
 	ret = next->len;
 	if (upperlen < ret) {
@@ -444,7 +498,7 @@
 	}
 	
 	if ((iovcnt > 0) && (iov->iov_len > 10)) {
-		LOG_A("Sending DTLS data, type %02hhx, Seq %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx, Stream %hu",
+		LOG_A("Sending DTLS data, type %hhd, Seq %02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx, Stream %hu",
 			((uint8_t *)iov->iov_base)[0],
 			((uint8_t *)iov->iov_base)[3],((uint8_t *)iov->iov_base)[4],((uint8_t *)iov->iov_base)[5],((uint8_t *)iov->iov_base)[6],
 			((uint8_t *)iov->iov_base)[7],((uint8_t *)iov->iov_base)[8],((uint8_t *)iov->iov_base)[9],((uint8_t *)iov->iov_base)[10],