diff -r 1e55e1aa8b7f sys/conf/files
--- a/sys/conf/files	Sun May 04 01:33:02 2014 +0000
+++ b/sys/conf/files	Fri Jul 11 13:32:06 2014 +1000
@@ -3324,6 +3324,7 @@
 netinet/raw_ip.c		optional inet | inet6
 netinet/cc/cc.c			optional inet | inet6
 netinet/cc/cc_newreno.c		optional inet | inet6
+netinet/mptcp_subr.c 		optional inet | inet6
 netinet/sctp_asconf.c		optional inet sctp | inet6 sctp
 netinet/sctp_auth.c		optional inet sctp | inet6 sctp
 netinet/sctp_bsd_addr.c		optional inet sctp | inet6 sctp
diff -r 1e55e1aa8b7f sys/conf/kern.pre.mk
--- a/sys/conf/kern.pre.mk	Sun May 04 01:33:02 2014 +0000
+++ b/sys/conf/kern.pre.mk	Fri Jul 11 13:32:06 2014 +1000
@@ -37,9 +37,9 @@
 .endif
 .if ${MACHINE_CPUARCH} == "amd64"
 .if ${COMPILER_TYPE} != "clang"
-COPTFLAGS?=-O2 -frename-registers -pipe
+COPTFLAGS?=-O0 -frename-registers -pipe
 .else
-COPTFLAGS?=-O2 -pipe
+COPTFLAGS?=-O0 -pipe
 .endif
 .else
 COPTFLAGS?=${_MINUS_O} -pipe
diff -r 1e55e1aa8b7f sys/kern/uipc_sockbuf.c
--- a/sys/kern/uipc_sockbuf.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/kern/uipc_sockbuf.c	Fri Jul 11 13:32:06 2014 +1000
@@ -570,8 +570,8 @@
 			mbcnt += m->m_ext.ext_size;
 	    }
 	}
-	if (len != sb->sb_cc || mbcnt != sb->sb_mbcnt) {
-		printf("cc %ld != %u || mbcnt %ld != %u\n", len, sb->sb_cc,
+	if (len != sb->sb_actual || mbcnt != sb->sb_mbcnt) {
+		printf("sb_actual %ld != %u || mbcnt %ld != %u\n", len, sb->sb_actual,
 		    mbcnt, sb->sb_mbcnt);
 		panic("sbcheck");
 	}
@@ -806,7 +806,20 @@
 			bcopy(mtod(m, caddr_t), mtod(n, caddr_t) + n->m_len,
 			    (unsigned)m->m_len);
 			n->m_len += m->m_len;
-			sb->sb_cc += m->m_len;
+			sb->sb_actual += m->m_len;
+			//sb->sb_cc += m->m_len;
+
+			atomic_add_acq_int(&sb->sb_cc, m->m_len);
+
+//			if (in_def_drop)
+//				printf("%s: while in in_deferred_drop, %d bytes\n", __func__,
+//				    m->m_len);
+
+			if(sb->sb_flags & SB_DEFERACCT)
+				KASSERT(sb->sb_cc <= sb->sb_actual, ("sb_cc greater than actual\n"));
+			else
+				KASSERT(sb->sb_cc == sb->sb_actual, ("SB_DEFER sb_cc greater != actual\n"));
+
 			if (m->m_type != MT_DATA && m->m_type != MT_OOBDATA)
 				/* XXX: Probably don't need.*/
 				sb->sb_ctl += m->m_len;
@@ -843,19 +856,26 @@
 		 * Don't call sbcut(sb, 0) if the leading mbuf is non-empty:
 		 * we would loop forever. Panic instead.
 		 */
-		if (!sb->sb_cc && (sb->sb_mb == NULL || sb->sb_mb->m_len))
+		if (!sb->sb_actual && (sb->sb_mb == NULL || sb->sb_mb->m_len))
 			break;
-		m_freem(sbcut_internal(sb, (int)sb->sb_cc));
+
+		/* This is a hack to adjuct sb_cc in cases where we want to
+		 * flush the buffer but have been doing deferred accounting.
+		 * this will ensure both sb_cc and sb_actual are zeroed when
+		 * the buffer is flushed */
+		if (sb->sb_flags & SB_DEFERACCT) {
+			sb->sb_cc = 0;
+		}
+		m_freem(sbcut_internal(sb, (int)sb->sb_actual));
 	}
-	if (sb->sb_cc || sb->sb_mb || sb->sb_mbcnt)
-		panic("sbflush_internal: cc %u || mb %p || mbcnt %u",
-		    sb->sb_cc, (void *)sb->sb_mb, sb->sb_mbcnt);
+	if (sb->sb_actual || sb->sb_cc || sb->sb_mb || sb->sb_mbcnt)
+		panic("sbflush_internal: sb_actual %u || sb_cc %u || mb %p || mbcnt %u",
+		    sb->sb_actual, sb->sb_cc, (void *)sb->sb_mb, sb->sb_mbcnt);
 }
 
 void
 sbflush_locked(struct sockbuf *sb)
 {
-
 	SOCKBUF_LOCK_ASSERT(sb);
 	sbflush_internal(sb);
 }
@@ -863,7 +883,6 @@
 void
 sbflush(struct sockbuf *sb)
 {
-
 	SOCKBUF_LOCK(sb);
 	sbflush_locked(sb);
 	SOCKBUF_UNLOCK(sb);
@@ -883,7 +902,7 @@
 	while (len > 0) {
 		if (m == 0) {
 			if (next == 0)
-				panic("sbdrop");
+				panic("%s: Not enough mbufs for specified drop length", __func__);
 			m = next;
 			next = m->m_nextpkt;
 			continue;
@@ -891,7 +910,21 @@
 		if (m->m_len > len) {
 			m->m_len -= len;
 			m->m_data += len;
-			sb->sb_cc -= len;
+
+			if(sb->sb_flags & SB_DEFERACCT)
+				KASSERT((int)sb->sb_actual - len >= 0 && sb->sb_cc <= sb->sb_actual - len,
+				    ("SB_DEFERACCT sb_cc/sb_actual accounting error: "
+				    		"sb_cc %u, sb_actual %u, len %d", sb->sb_cc, sb->sb_actual, len));
+			else
+				KASSERT((int)sb->sb_cc - len >= 0 && (int)sb->sb_actual
+				    - len >= 0 && sb->sb_cc - len <= sb->sb_actual
+				    - len && sb->sb_cc == sb->sb_actual,
+				    ("sb_cc/sb_actual accounting error: sb_cc %u, sb_actual %u, "
+				    		"len %d", sb->sb_cc, sb->sb_actual, len));
+
+			sb->sb_actual -= len;
+			if (!(sb->sb_flags & SB_DEFERACCT))
+				sb->sb_cc -= len;
 			if (sb->sb_sndptroff != 0)
 				sb->sb_sndptroff -= len;
 			if (m->m_type != MT_DATA && m->m_type != MT_OOBDATA)
@@ -977,8 +1010,8 @@
 	struct mbuf *m, *ret;
 
 	KASSERT(sb->sb_mb != NULL, ("%s: sb_mb is NULL", __func__));
-	KASSERT(off + len <= sb->sb_cc, ("%s: beyond sb", __func__));
-	KASSERT(sb->sb_sndptroff <= sb->sb_cc, ("%s: sndptroff broken", __func__));
+	KASSERT(off + len <= sb->sb_actual, ("%s: beyond sb", __func__));						//XXX is this okay?
+	KASSERT(sb->sb_sndptroff <= sb->sb_actual, ("%s: sndptroff broken", __func__));			//XXX same here
 
 	/*
 	 * Is off below stored offset? Happens on retransmits.
@@ -998,6 +1031,7 @@
 		m = ret = m->m_next;
 		KASSERT(ret->m_len > 0,
 		    ("mbuf %p in sockbuf %p chain has no valid data", ret, sb));
+
 	}
 
 	/* Advance by len to be as close as possible for the next transmit. */
diff -r 1e55e1aa8b7f sys/kern/uipc_socket.c
--- a/sys/kern/uipc_socket.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/kern/uipc_socket.c	Fri Jul 11 13:32:06 2014 +1000
@@ -110,6 +110,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/fcntl.h>
+#include <sys/kdb.h>
 #include <sys/limits.h>
 #include <sys/lock.h>
 #include <sys/mac.h>
@@ -124,6 +125,7 @@
 #include <sys/poll.h>
 #include <sys/proc.h>
 #include <sys/protosw.h>
+#include <sys/queue.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/resourcevar.h>
@@ -139,6 +141,11 @@
 
 #include <net/vnet.h>
 
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
+
 #include <security/mac/mac_framework.h>
 
 #include <vm/uma.h>
@@ -1749,6 +1756,7 @@
 				m->m_data += len;
 				m->m_len -= len;
 				so->so_rcv.sb_cc -= len;
+				so->so_rcv.sb_actual -= len;
 			}
 		}
 		SOCKBUF_LOCK_ASSERT(&so->so_rcv);
@@ -1963,6 +1971,7 @@
 	goto restart;
 
 deliver:
+
 	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
 	KASSERT(sb->sb_cc > 0, ("%s: sockbuf empty", __func__));
 	KASSERT(sb->sb_mb != NULL, ("%s: sb_mb == NULL", __func__));
@@ -2058,6 +2067,225 @@
 	return (error);
 }
 
+
+/*
+ *  Modified version of soreceive_stream() for Multipath-enabled
+ *  connections
+ */
+int
+soreceive_mpstream(struct socket *so, struct sockaddr **psa, struct uio *uio,
+    struct mbuf **mp0, struct mbuf **controlp, int *flagsp)
+{
+	int len = 0, error = 0, flags, oresid;
+	struct mbuf *n = NULL, *m;
+	struct mpcb *mp;
+	struct sockbuf *sb;
+	struct tcpcb *tp = sototcpcb(so);
+
+	if(!tp)
+		return(EINVAL);
+
+	mp = tp->t_mpcb;
+	KASSERT(mp != NULL, ("%s Doing mpstream re-assembly without a valid mp block\n" ,__func__));
+
+	/* We only do stream sockets. */
+	if (so->so_type != SOCK_STREAM)
+		return (EINVAL);
+	if (psa != NULL)
+		*psa = NULL;
+	if (controlp != NULL)
+		return (EINVAL);
+	if (flagsp != NULL)
+		flags = *flagsp &~ MSG_EOR;
+	else
+		flags = 0;
+	if (flags & MSG_OOB)
+		return (soreceive_rcvoob(so, uio, flags));
+	if (mp0 != NULL)
+		*mp0 = NULL;
+
+	sb = &so->so_rcv;
+
+	/* Prevent other readers from entering the socket. */
+	error = sblock(sb, SBLOCKWAIT(flags));
+	if (error)
+		goto out;
+	SOCKBUF_LOCK(sb);
+
+	//mp_do_reass(so);
+
+	/* Easy one, no space to copyout anything. */
+	if (uio->uio_resid == 0) {
+		error = EINVAL;
+		goto out;
+	}
+	oresid = uio->uio_resid;
+
+	/* We will never ever get anything unless we are or were connected. */
+	if (!(so->so_state & (SS_ISCONNECTED|SS_ISDISCONNECTED))) {
+		error = ENOTCONN;
+		goto out;
+	}
+
+restart:
+	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
+
+	/* Abort if socket has reported problems. */
+	if (so->so_error) {
+		if (sb->sb_cc > 0)
+			goto deliver;
+		if (oresid > uio->uio_resid)
+			goto out;
+		error = so->so_error;
+		if (!(flags & MSG_PEEK))
+			so->so_error = 0;
+		goto out;
+	}
+
+	/* Door is closed.  Deliver what is left, if any. */
+	if (sb->sb_state & SBS_CANTRCVMORE) {
+		if (sb->sb_cc > 0)
+			goto deliver;
+		else
+			goto out;
+	}
+
+	/*
+	 * If the last received sequence is less than what
+	 * was expected.
+	 */
+//	if (tp->mp_enabled) {
+//		if ((mp->ds_rcv_nxt - mp->ds_last_rcvd ) >= 0)
+//			goto out;
+//	}
+
+	/* Socket buffer is empty and we shall not block. */
+	if (sb->sb_cc == 0 &&
+	    ((so->so_state & SS_NBIO) || (flags & (MSG_DONTWAIT|MSG_NBIO)))) {
+		error = EAGAIN;
+		goto out;
+	}
+
+	/* Socket buffer got some data that we shall deliver now. */
+	if (sb->sb_cc > 0 && !(flags & MSG_WAITALL) &&
+	    ((sb->sb_flags & SS_NBIO) ||
+	     (flags & (MSG_DONTWAIT|MSG_NBIO)) ||
+	     sb->sb_cc >= sb->sb_lowat ||
+	     sb->sb_cc >= uio->uio_resid ||
+	     sb->sb_cc >= sb->sb_hiwat) ) {
+		goto deliver;
+	}
+
+	/* On MSG_WAITALL we must wait until all data or error arrives. */
+	if ((flags & MSG_WAITALL) &&
+	    (sb->sb_cc >= uio->uio_resid || sb->sb_cc >= sb->sb_lowat))
+		goto deliver;
+
+	/*
+	 * Wait and block until (more) data comes in.
+	 * NB: Drops the sockbuf lock during wait.
+	 */
+	error = sbwait(sb);
+	if (error)
+		goto out;
+	goto restart;
+
+deliver:
+	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
+	KASSERT(sb->sb_cc > 0, ("%s: sockbuf empty", __func__));
+	KASSERT(sb->sb_mb != NULL, ("%s: sb_mb == NULL", __func__));
+
+	/* Statistics. */
+	if (uio->uio_td)
+		uio->uio_td->td_ru.ru_msgrcv++;
+
+	/* Fill uio until full or current end of socket buffer is reached. */
+	len = min(uio->uio_resid, sb->sb_cc);
+
+//printf("%s: deliver len %d\n",__func__, len);
+	if (mp0 != NULL) {
+		/* Dequeue as many mbufs as possible. */
+		if (!(flags & MSG_PEEK) && len >= sb->sb_mb->m_len) {
+			for (*mp0 = m = sb->sb_mb;
+			     m != NULL && m->m_len <= len;
+			     m = m->m_next) {
+				len -= m->m_len;
+				uio->uio_resid -= m->m_len;
+				sbfree(sb, m);
+				n = m;
+			}
+			sb->sb_mb = m;
+			if (sb->sb_mb == NULL)
+				SB_EMPTY_FIXUP(sb);
+			n->m_next = NULL;
+		}
+		/* Copy the remainder. */
+		if (len > 0) {
+			KASSERT(sb->sb_mb != NULL,
+			    ("%s: len > 0 && sb->sb_mb empty", __func__));
+
+			m = m_copym(sb->sb_mb, 0, len, M_NOWAIT);
+			if (m == NULL)
+				len = 0;	/* Don't flush data from sockbuf. */
+			else
+				uio->uio_resid -= m->m_len;
+			if (*mp0 != NULL)
+				n->m_next = m;
+			else
+				*mp0 = m;
+			if (*mp0 == NULL) {
+				error = ENOBUFS;
+				goto out;
+			}
+		}
+	} else {
+		/* NB: Must unlock socket buffer as uiomove may sleep. */
+
+		SOCKBUF_UNLOCK(sb);
+		error = m_mbuftouio(uio, sb->sb_mb, len);
+		SOCKBUF_LOCK(sb);
+
+		if (error)
+			goto out;
+	}
+	SBLASTRECORDCHK(sb);
+	SBLASTMBUFCHK(sb);
+
+	/*
+	 * Remove the delivered data from the socket buffer unless we
+	 * were only peeking.
+	 */
+	if (!(flags & MSG_PEEK)) {
+		if (len > 0)
+			sbdrop_locked(sb, len);
+
+		/* Notify protocol that we drained some data. */
+		if ((so->so_proto->pr_flags & PR_WANTRCVD) &&
+		    (((flags & MSG_WAITALL) && uio->uio_resid > 0) ||
+		     !(flags & MSG_SOCALLBCK))) {
+			SOCKBUF_UNLOCK(sb);
+			VNET_SO_ASSERT(so);
+			(*so->so_proto->pr_usrreqs->pru_rcvd)(so, flags);
+			SOCKBUF_LOCK(sb);
+		}
+	}
+
+	/*
+	 * For MSG_WAITALL we may have to loop again and wait for
+	 * more data to come in.
+	 */
+	if ((flags & MSG_WAITALL) && uio->uio_resid > 0)
+		goto restart;
+out:
+
+	SOCKBUF_LOCK_ASSERT(sb);
+	SBLASTRECORDCHK(sb);
+	SBLASTMBUFCHK(sb);
+	SOCKBUF_UNLOCK(sb);
+	sbunlock(sb);
+	return (error);
+}
+
 /*
  * Optimized version of soreceive() for simple datagram cases from userspace.
  * Unlike in the stream case, we're able to drop a datagram if copyout()
@@ -2952,6 +3180,52 @@
 }
 
 int
+sopoll_mpstream(struct socket *so, int events, struct ucred *active_cred,
+    struct thread *td)
+{
+	int revents = 0;
+
+	SOCKBUF_LOCK(&so->so_snd);
+	SOCKBUF_LOCK(&so->so_rcv);
+	if (events & (POLLIN | POLLRDNORM))
+		if (soreadabledata(so))
+			revents |= events & (POLLIN | POLLRDNORM);
+
+	if (events & (POLLOUT | POLLWRNORM))
+		if (sowriteable(so))
+			revents |= events & (POLLOUT | POLLWRNORM);
+
+	if (events & (POLLPRI | POLLRDBAND))
+		if (so->so_oobmark || (so->so_rcv.sb_state & SBS_RCVATMARK))
+			revents |= events & (POLLPRI | POLLRDBAND);
+
+	if ((events & POLLINIGNEOF) == 0) {
+		if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
+			revents |= events & (POLLIN | POLLRDNORM);
+			if (so->so_snd.sb_state & SBS_CANTSENDMORE)
+				revents |= POLLHUP;
+		}
+	}
+
+	if (revents == 0) {
+		if (events & (POLLIN | POLLPRI | POLLRDNORM | POLLRDBAND)) {
+			selrecord(td, &so->so_rcv.sb_sel);
+			so->so_rcv.sb_flags |= SB_SEL;
+		}
+
+		if (events & (POLLOUT | POLLWRNORM)) {
+			selrecord(td, &so->so_snd.sb_sel);
+			so->so_snd.sb_flags |= SB_SEL;
+		}
+	}
+
+	SOCKBUF_UNLOCK(&so->so_rcv);
+	SOCKBUF_UNLOCK(&so->so_snd);
+	return (revents);
+}
+
+
+int
 soo_kqfilter(struct file *fp, struct knote *kn)
 {
 	struct socket *so = kn->kn_fp->f_data;
@@ -3350,7 +3624,8 @@
 	sorwakeup_locked(so);
 	SOCKBUF_LOCK(&so->so_snd);
 	so->so_snd.sb_state |= SBS_CANTSENDMORE;
-	sbdrop_locked(&so->so_snd, so->so_snd.sb_cc);
+//	sbdrop_locked(&so->so_snd, so->so_snd.sb_actual);
+	sbflush_locked(&so->so_snd);
 	sowwakeup_locked(so);
 	wakeup(&so->so_timeo);
 }
diff -r 1e55e1aa8b7f sys/netinet/in_pcb.c
--- a/sys/netinet/in_pcb.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/in_pcb.c	Fri Jul 11 13:32:06 2014 +1000
@@ -322,6 +322,69 @@
 	return (error);
 }
 
+/*
+ * Allocate a PCB and associate it with the socket.
+ * On success return with the PCB locked.
+ */
+struct inpcb *
+in_pcballoc_subflow(struct socket *so, struct inpcbinfo *pcbinfo)
+{
+	struct inpcb *inp;
+	int error;
+
+	INP_INFO_WLOCK_ASSERT(pcbinfo);
+	error = 0;
+	inp = uma_zalloc(pcbinfo->ipi_zone, M_NOWAIT);
+	if (inp == NULL)
+		return inp;
+
+	bzero(inp, inp_zero_size);
+	inp->inp_pcbinfo = pcbinfo;
+	inp->inp_socket = so;
+	inp->inp_cred = crhold(so->so_cred);
+	inp->inp_inc.inc_fibnum = so->so_fibnum;
+#ifdef MAC
+	error = mac_inpcb_init(inp, M_NOWAIT);
+	if (error != 0)
+		goto out;
+	mac_inpcb_create(so, inp);
+#endif
+#ifdef IPSEC
+	error = ipsec_init_policy(so, &inp->inp_sp);
+	if (error != 0) {
+#ifdef MAC
+		mac_inpcb_destroy(inp);
+#endif
+		goto out;
+	}
+#endif /*IPSEC*/
+#ifdef INET6
+	if (INP_SOCKAF(so) == AF_INET6) {
+		inp->inp_vflag |= INP_IPV6PROTO;
+		if (V_ip6_v6only)
+			inp->inp_flags |= IN6P_IPV6_V6ONLY;
+	}
+#endif
+	LIST_INSERT_HEAD(pcbinfo->ipi_listhead, inp, inp_list);
+	pcbinfo->ipi_count++;
+#ifdef INET6
+	if (V_ip6_auto_flowlabel)
+		inp->inp_flags |= IN6P_AUTOFLOWLABEL;
+#endif
+	INP_WLOCK(inp);
+	inp->inp_gencnt = ++pcbinfo->ipi_gencnt;
+	refcount_init(&inp->inp_refcount, 1);	/* Reference from inpcbinfo */
+#if defined(IPSEC) || defined(MAC)
+out:
+	if (error != 0) {
+		crfree(inp->inp_cred);
+		uma_zfree(pcbinfo->ipi_zone, inp);
+		inp = NULL;
+	}
+#endif
+	return (inp);
+}
+
 #ifdef INET
 int
 in_pcbbind(struct inpcb *inp, struct sockaddr *nam, struct ucred *cred)
@@ -1027,6 +1090,7 @@
 		if (error)
 			return (error);
 	}
+
 	oinp = in_pcblookup_hash_locked(inp->inp_pcbinfo, faddr, fport,
 	    laddr, lport, 0, NULL);
 	if (oinp != NULL) {
diff -r 1e55e1aa8b7f sys/netinet/in_pcb.h
--- a/sys/netinet/in_pcb.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/in_pcb.h	Fri Jul 11 13:32:06 2014 +1000
@@ -619,6 +619,8 @@
 
 void	in_pcbpurgeif0(struct inpcbinfo *, struct ifnet *);
 int	in_pcballoc(struct socket *, struct inpcbinfo *);
+struct inpcb *
+	in_pcballoc_subflow(struct socket *so, struct inpcbinfo *pcbinfo);
 int	in_pcbbind(struct inpcb *, struct sockaddr *, struct ucred *);
 int	in_pcb_lport(struct inpcb *, struct in_addr *, u_short *,
 	    struct ucred *, int);
diff -r 1e55e1aa8b7f sys/netinet/mptcp.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp.h	Fri Jul 11 13:32:06 2014 +1000
@@ -0,0 +1,89 @@
+/*-
+ * Copyright (c) 2012-2013
+ * 	Swinburne University of Technology, Melbourne, Australia.
+ * All rights reserved.
+ *
+ * This software was developed at the Centre for Advanced Internet
+ * Architectures, Swinburne University of Technology, by Nigel Williams, made
+ * possible in part by a gift from The Cisco University Research Program Fund,
+ * a corporate advised fund of Silicon Valley Community Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * mptcp.h
+ *
+ *  Created on: 15/05/2012
+ *      Author: nwilliams
+ */
+
+#ifndef MPTCP_H_
+#define MPTCP_H_
+
+
+#include <sys/cdefs.h>
+
+#define MPTCP_64BIT_KEY 8
+
+typedef	u_int64_t mptcp_seq;
+
+/* MPTCP subtypes */
+#define	MPTCP_SUBTYPE_MP_CAPABLE	0
+#define		MPTCP_SUBLEN_MP_CAPABLE_SYN	12
+#define		MPTCP_SUBLEN_MP_CAPABLE_ACK	20
+
+#define MPTCP_SUBTYPE_MP_JOIN		1
+#define		MPTCP_SUBLEN_MP_JOIN_SYN	12
+#define		MPTCP_SUBLEN_MP_JOIN_SYNACK	16		// should be 16, but run out of option space
+#define		MPTCP_SUBLEN_MP_JOIN_ACK	24		// should be 24, but run out of option space
+
+#define MPTCP_SUBTYPE_DSS			2
+#define		MPTCP_SUBLEN_DSS_DATA_ACK	XX
+#define		MPTCP_SUBLEN_DSS_DATA_DSN	XX
+
+#define MPTCP_SUBTYPE_ADD_ADDR		3
+#define 	MPTCP_SUBLEN_ADD_ADDRV4		8
+#define 	MPTCP_SUBLEN_ADD_ADDRV6		20
+
+#define MPTCP_SUBTYPE_REMOVE_ADDR	4
+#define 	MPTCP_SUBLEN_REMOVE_ADDR	4
+
+#define MPTCP_SUBTYPE_MP_PRIO		5
+
+#define MPTCP_SUBTYPE_MP_FAIL		6
+#define		MPTCP_SUBTYPELEN_MP_FAIL	12
+
+#define MPTCP_SUBTYPE_MP_FASTCLOSE	7
+#define 	MPTCP_SUBTYPELEN_MP_FASTCLOSE 12
+
+#define	MAX_MP_OPLEN	28
+
+/* mptcp errors */
+
+#define EMAXSUBFLOWSREACHED 01
+#define ENOMPCB	02
+#define	ENOTCPCB 03
+
+/* mptcp funcs */
+
+#endif /* MPTCP_H_ */
diff -r 1e55e1aa8b7f sys/netinet/mptcp_subr.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_subr.c	Fri Jul 11 13:32:06 2014 +1000
@@ -0,0 +1,1920 @@
+/*-
+ * Copyright (c) 2012-2013
+ * 	Swinburne University of Technology, Melbourne, Australia.
+ * All rights reserved.
+ *
+ * This software was developed at the Centre for Advanced Internet
+ * Architectures, Swinburne University of Technology, by Nigel Williams and
+ * Lawrence Stewart, made possible in part by a gift from The Cisco University
+ * Research Program Fund, a corporate advised fund of Silicon Valley Community
+ * Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "opt_compat.h"
+#include "opt_inet.h"
+#include "opt_inet6.h"
+#include "opt_ipsec.h"
+#include "opt_tcpdebug.h"
+
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/param.h>
+#include <sys/queue.h>
+#include <sys/systm.h>
+#include <sys/callout.h>
+#include <sys/hhook.h>
+#include <sys/kernel.h>
+#include <sys/khelp.h>
+#include <sys/kdb.h>
+#include <sys/lock.h>
+#include <sys/rwlock.h>
+#include <sys/sysctl.h>
+#include <sys/sbuf.h>
+#include <sys/taskqueue.h>
+#include <sys/jail.h>
+
+#ifdef INET6
+#include <sys/domain.h>
+#endif
+#include <sys/priv.h>
+#include <sys/proc.h>
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/protosw.h>
+#include <sys/random.h>
+
+/* for SCTP auth functions */
+#include <crypto/sha1.h>
+
+/* for checking interface status */
+#include <net/if.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_fsm.h>
+
+#include <netinet/mptcp_var.h>
+
+/* for SCTP auth functions */
+#include <netinet/sctp_pcb.h>
+#include <netinet/sctp_auth.h>
+#include <netinet/sctp_constants.h>
+
+#include <machine/stdarg.h>
+
+/* Declare sysctl tree and populate it. */
+SYSCTL_NODE(_net_inet_tcp, OID_AUTO, mptcp, CTLFLAG_RW, NULL,
+    "Mulitpath TCP related settings");
+
+VNET_DEFINE(int, linux_compat) = 1;
+#define	V_mptcp_linux_compat	VNET(mptcp_linux_compat);
+SYSCTL_VNET_INT(_net_inet_tcp_mptcp, OID_AUTO, linux_compat, CTLFLAG_RW,
+    &VNET_NAME(linux_compat), 0,
+    "Subtle changes for linux mptcp interop");
+
+VNET_DEFINE(int, max_subflows) = 8;
+#define	V_mptcp_max_subflows	VNET(mptcp_max_subflows);
+SYSCTL_VNET_INT(_net_inet_tcp_mptcp, OID_AUTO, max_subflows, CTLFLAG_RW,
+    &VNET_NAME(max_subflows), 0,
+    "Maximum number of subflows per Multipath TCP Connection");
+
+VNET_DEFINE(int, single_packet_maps) = 1;
+#define	V_mptcp_single_packet_maps	VNET(mptcp_single_packet_maps);
+SYSCTL_VNET_INT(_net_inet_tcp_mptcp, OID_AUTO, single_packet_maps, CTLFLAG_RW,
+    &VNET_NAME(single_packet_maps), 0,
+    "DSN Maps cover a single packet only");
+
+
+struct debug_class {
+	char class[32];
+	int verbosity;
+	int loglevelexclusive;
+};
+
+uint32_t mp_active_debug_classes;
+
+struct debug_class debug_classes[] = {
+		{.class = "MPSESSION"},
+		{.class = "DSMAP"},
+		{.class = "SBSTATUS"},
+		{.class = "REASS"}
+};
+
+#define	N_DEBUGCLASSES (sizeof(debug_classes)/sizeof(struct debug_class))
+
+
+static void mp_drop_dup_maps(struct mpcb *mp);
+
+//struct ds_map*
+//mp_get_dsmap(struct tcpcb *tp, uint64_t ds_ack_num);
+
+/* keeping track of addresses added via sysctl:
+ * net.inet.tcp.mptcp.mp_addresses. Though we start at '1' as the 'master'
+ * address is included.
+ */
+int mp_address_count = 1;
+struct sockaddr_storage mp_usable_addresses[MAX_ADDRS];
+
+/* Initialise extern variables that store addresses */
+struct sockaddr_storage mp_path_manager[MAX_ADDRS];
+
+int in_def_drop = 0;
+
+#define MSBIT_CHECK 0x80000000
+
+static void	mp_update_recwin(struct mpcb *mp);
+//static void mp_alloc_pcbs(struct sockaddr_storage, struct inpcb *sf_inp, struct mpcb *mp);
+
+
+/* iterate through the list of tcpcbs and print the DSN, T_SEQ of the segs
+ * in reassembly
+ * Calling from the debugger so not worried about locking
+ */
+void
+mp_print_reass_lists(struct mpcb *mp) {
+	struct tcpcb *sf_tp;
+	struct tseg_qent *q = NULL;
+	int index = 0;
+
+//	mp_print_sqns(mp);
+
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		index = 0;
+		TAILQ_FOREACH(q, sf_tp->t_segq, tqe_q) {
+			printf("%d: DSN: %u, TSEQ: %u, len %d\n", index,
+			    (uint32_t) q->tqe_dsn, q->tqe_th->th_seq, q->tqe_len);
+			    index++;
+		}
+		printf("----\n");
+	}
+}
+
+/* print the ds_level sequence numbers */
+void
+mp_print_sqns(struct mpcb *mp) {
+	printf("ds_rcv_nxt: %u\n", (uint32_t) mp->ds_rcv_nxt);
+}
+
+/* print rcvd mappings */
+void
+mp_print_maps(struct mpcb *mp) {
+
+}
+
+void
+mp_debug(uint32_t log_class, int msg_verbosity, uint32_t flags, char * fmt, ...) {
+	va_list ap;
+	int effective_verbosity, i, loglevelexclusive;
+
+	effective_verbosity = 0;
+
+	if (log_class & mp_active_debug_classes) {
+	// calculate shift index (index of class into array)
+		i = 0;
+		loglevelexclusive = 0;
+		while(log_class >= 1) {
+			if (log_class & 0x1) {
+				effective_verbosity = max(debug_classes[i].verbosity, effective_verbosity);
+				loglevelexclusive += debug_classes[i].loglevelexclusive;
+			}
+			log_class >>= 1;
+			i++;
+		}
+
+		if	((loglevelexclusive && msg_verbosity == effective_verbosity) ||
+				(!loglevelexclusive && msg_verbosity <= effective_verbosity)) {
+			va_start(ap, fmt);
+			vprintf(fmt, ap);
+			va_end(ap);
+		}
+	}
+}
+
+/*
+ * Sysctl handler to show and change slave subflow addresses
+ */
+static int
+mp_addresses(SYSCTL_HANDLER_ARGS)
+{
+	char *straddr;
+	struct sbuf *s;
+	void *addr;
+	char inet_buf[64];
+	int error, i, ret;
+
+	error = 0;
+
+	if (req->newptr == NULL) {
+
+		s = sbuf_new_for_sysctl(NULL, NULL, 300, req);
+		if (s == NULL)
+			return (ENOMEM);
+
+		/* Start at i = 1 as the 'master' address is always included
+		 * by default */
+		for (i = 1; i < mp_address_count; i++) {
+			switch (mp_usable_addresses[i].ss_family) {
+				case AF_INET:
+					addr = &((struct sockaddr_in *)
+					    &mp_usable_addresses[i])->sin_addr;
+					break;
+				case AF_INET6:
+					addr = &((struct sockaddr_in6 *)
+					    &mp_usable_addresses[i])->sin6_addr;
+					break;
+				default:
+					return (EINVAL);
+			}
+
+			inet_ntop(mp_usable_addresses[i].ss_family, addr, inet_buf,
+			    mp_usable_addresses[i].ss_len);
+			sbuf_cat(s, inet_buf);
+			if (i < (mp_address_count - 1))
+				sbuf_cat(s, " ");
+		}
+
+		error = sbuf_finish(s);
+		sbuf_delete(s);
+	} else {
+		i = 1;
+		while ((straddr = strsep((char **)&req->newptr, " "))
+			!= NULL && i < MAX_ADDRS && !error) {
+			ret = inet_pton(AF_INET6, straddr,
+			    &((struct sockaddr_in6 *)&mp_usable_addresses[i])->sin6_addr);
+			if (ret == 1) {
+				mp_usable_addresses[i].ss_family = AF_INET6;
+				mp_usable_addresses[i].ss_len = sizeof(struct sockaddr_in6);
+			} else if (ret == 0) {
+				ret = inet_pton(AF_INET, straddr,
+				&((struct sockaddr_in *)&mp_usable_addresses[i])->sin_addr);
+				if (ret == 1) {
+					mp_usable_addresses[i].ss_family = AF_INET;
+					mp_usable_addresses[i].ss_len = sizeof(struct sockaddr_in);
+				}
+			}
+
+			if (ret < 1) {
+				char *zero = "0";
+				mp_address_count = 1; //XXXNJW: Shouldn't be reading into mp_usable_addresses directly...
+				if (strcmp(straddr, zero) == 0)
+					return(0);
+				error = EINVAL;
+			}
+			i++;
+		}
+
+		if (!error) {
+			if (i == mp_address_count)
+				error = E2BIG;
+			else
+				mp_address_count = i;
+		}
+	}
+
+	return (error);
+}
+
+/*
+ * Sysctl handler for setting debug output levels for mp
+ */
+static int
+mp_debug_sysctl_handler(SYSCTL_HANDLER_ARGS)
+{
+	struct sbuf *s;
+	int error, i;
+
+	error = 0;
+
+	if (req->newptr == NULL) {
+		s = sbuf_new_for_sysctl(NULL, NULL, 300, req);
+		if (s == NULL)
+			return (ENOMEM);
+
+		for(i = 0; i < N_DEBUGCLASSES; i++) {
+				if (i > 0)
+					sbuf_putc(s, ',');
+				sbuf_printf(s, "%s:%c%d", debug_classes[i].class,
+					debug_classes[i].loglevelexclusive ? '=' : '*',
+					debug_classes[i].verbosity);
+		}
+
+		error = sbuf_finish(s);
+		sbuf_delete(s);
+	} else {
+		char *class, *pair;
+
+		while ((pair = strsep(((char **)&req->newptr), ",")) != NULL) {
+			class = strsep(&pair, ":");
+			for(i = 0; i < N_DEBUGCLASSES; i++) {
+				if (strcmp(class, debug_classes[i].class) == 0
+					|| strcmp(class, "ALL") == 0) {
+					if (pair[0] == '=') {
+						debug_classes[i].loglevelexclusive = 1;
+						pair++;
+					} else {
+						if (pair[0] == '*')
+							pair++;
+						debug_classes[i].loglevelexclusive = 0;
+					}
+					debug_classes[i].verbosity = strtol(pair, NULL, 10);
+					if (debug_classes[i].verbosity == 0)
+						mp_active_debug_classes &= ~(1<<i);
+					else
+						mp_active_debug_classes |= 1<<i;
+				}
+			}
+		}
+	}
+
+	return (error);
+}
+
+/*
+ * Some basic random number/hashing functions that should
+ * be okay for initial release
+ *
+ * XXXNJW: Need a more secure method?
+ */
+u_int64_t
+mp_generate_local_key(void)
+{
+       u_int64_t key = arc4random();
+       key = key << 32;
+       key += arc4random();
+       return key;
+}
+
+/*
+ * Digest is for a 20 byte SHA1, key is the session key created
+ * when the mpcb was created
+ */
+uint32_t
+mp_do_sha_hash(uint8_t *digest, uint8_t *key, uint64_t key_length) {
+
+       SHA1_CTX context;
+
+       if ((key == NULL) || (key_length == 0)) {
+              return (0);
+       }
+
+       SHA1Init(&context);
+       SHA1Update(&context, key, key_length);
+       SHA1Final(digest, &context);
+
+       return key_length;
+}
+
+// XXXNJW: turn these into Macros
+/*
+ * Take a digest and return a token. Linux_compat
+ * changes the offset into the digest.
+ */
+uint32_t
+mp_get_token(uint8_t *digest) {
+	if(!linux_compat)
+		return (*((uint32_t *) (digest + 16)));
+	else
+		return (*((uint32_t *) (digest)));
+}
+
+/*
+ * Take a digest and return the idsn. Linux_compat
+ * changes the offset into the digest.
+ */
+
+uint64_t
+mp_new_idsn(uint8_t *digest) {
+	if(!linux_compat)
+		return (*((uint64_t *) (digest)));
+	else
+		return (*((uint64_t *) (digest+12)));
+}
+
+/*
+ * HMAC for used when completing mp_join handshake.
+ * We use the hmac functions implemented in sctp_auth.c
+ */
+uint32_t
+mp_get_hmac(uint8_t *digest, uint64_t local_key, uint64_t remote_key,
+		uint32_t local_rand, uint32_t remote_rand) {
+	/*
+	* Set up the hmac key
+	*/
+	uint8_t key[16];
+	uint32_t keylen = sizeof(local_key) + sizeof(remote_key);
+
+	bcopy(&local_key, key, sizeof(local_key));
+	bcopy(&remote_key, key + 8, sizeof(remote_key));
+
+	/*
+	* Set up the hmac msg
+	*/
+	uint64_t hmac_msg;
+	hmac_msg = (uint64_t)local_rand << 32;
+	hmac_msg |= remote_rand;
+
+	/*
+	 * Do the hashing. 0x0001 is the type for sha1
+	 */
+	uint32_t hmac_len = sctp_hmac(0x0001, key, keylen,
+	    (uint8_t *) &hmac_msg, sizeof(hmac_msg), digest);
+
+	return(hmac_len);
+}
+
+/*
+ * Debugging function for printing a sequence of bytes
+ */
+void
+btohex(char *buf, uint32_t buf_len, uint8_t *bytes, int32_t bytes_len, int32_t flags)
+{
+	struct sbuf out;
+	int32_t i;
+
+	i = bytes_len - 1;
+	sbuf_new(&out, buf, buf_len, 0);
+
+	while (i >= 0) {
+		if (flags & BTOHEX_MSBRTOL)
+			sbuf_printf(&out, "%0x", *(bytes + bytes_len - i - 1));
+		else
+			sbuf_printf(&out, "%0x", *(bytes + i));
+		i--;
+	}
+
+	sbuf_finish(&out);
+}
+
+/*
+ * Return the common rcv window. The window is updated when soreceive is called
+ * and is attached to the subsequent outgoing DACK that signifies that data
+ * has been read by the application.
+ */
+uint32_t
+mp_get_recwin(struct mpcb *mp) {
+	mp_update_recwin(mp);
+	return mp->ds_rcv_wnd;
+}
+
+static void
+mp_update_recwin(struct mpcb *mp) {
+	/* would an atomic_set_32 work here? or issues with SMP? */
+	MPCB_LOCK(mp);
+	mp->ds_rcv_wnd = sbspace(&mp->master_socket->so_rcv);
+	MPCB_UNLOCK(mp);
+}
+
+void
+mp_update_sndwin(struct mpcb *mp, uint16_t win) {
+	/* would an atomic_set_32 work here? or issues with SMP? */
+	MPCB_LOCK(mp);
+	mp->ds_snd_wnd = win;
+	MPCB_UNLOCK(mp);
+}
+
+int
+mp_close_subflows() {
+	//XXXNJW: tdb
+	return (0);
+}
+
+int
+mp_close_subflow(struct tcpcb *tp) {
+	//XXXNJW: tdb
+	return (0);
+}
+
+/* drop a subflow tcpcb from the list of subflows on this mp connection.
+ * XXXNJW: the locking situation here? */
+void
+mp_drop_subflow_tp(struct mpcb *mp, struct tcpcb *tp) {
+	mp_debug(MPSESSION, 1, 0, "%s: dropping tp %p from mp->tp_list\n",
+	    __func__, tp);
+	MPCB_LOCK(mp);
+	SLIST_REMOVE(&mp->tp_list, tp, tcpcb, next_subflow);
+	MPCB_UNLOCK(mp);
+}
+
+/*
+ * XXXNJW: To change. Currently used to call tcp_output on subflows that are
+ * 'waiting' for data (they didn't get an maps from the send buffer on the last
+ * call through).
+ */
+void
+mp_datascheduler_task_handler(void *context, int pending)
+{
+	struct mpcb *mp;
+	struct tcpcb *sf_tp;
+
+	mp = (struct mpcb *)context;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		INP_WLOCK(sf_tp->t_inpcb);
+		if (!(sf_tp->t_inpcb->inp_flags & INP_DROPPED)
+		    && (sf_tp->sf_flags & SFF_DATA_WAIT) && (sf_tp->t_rxtshift == 0)) {
+			tcp_output(sf_tp);
+			INP_WUNLOCK(sf_tp->t_inpcb);
+			break;
+		}
+		INP_WUNLOCK(sf_tp->t_inpcb);
+	}
+}
+
+void
+mp_trigger_rexmit(struct mpcb *mp) {
+	taskqueue_enqueue(taskqueue_swi, &mp->rexmit_task);
+}
+
+/*
+ * called when data-level ACK is outstanding. Queue up the maps to be rexmitted
+ * in a new list and schedule the rexmit task. The task handler will rexmit the
+ * maps on the first subflow it finds that isn't in subflow-level rexmit.
+ */
+void
+mp_queue_data_rexmit_unlocked(struct tcpcb *tp) {
+
+	struct mpcb *mp = tp->t_mpcb;
+
+	mp_debug(DSMAP, 2, 0, "%s: ds_snd_una: %u \n", __func__,
+	    (uint32_t) mp->ds_snd_una);
+
+	MPCB_LOCK(mp);
+
+	/* XXXNJW: to keep things simple, if we already have maps to retransmit,
+	 * then continue trying with those rather than adding new ones
+	 *
+	 * if we have maps that have been sent, start above ds_snd_una and have
+	 * not been acked at the subflow level, then we should try to retransmit.
+	 * place these maps into mp_rxmitmaps. When tcp_output is called next,
+	 * the to-retransmit maps will be returned.
+	 */
+	if (TAILQ_EMPTY(&mp->mp_rxtmitmaps)) {
+		struct ds_map *sent_map;
+		// modified to look for rexmits only for tp that
+		// called the function (from tcp_timer)
+		// won't attempt to re-enqueue a segment that was a retransmit of
+		// another segment
+		TAILQ_FOREACH(sent_map, &tp->t_txdsmaps, sf_ds_map_next) {
+			if ((sent_map->ds_map_start >= tp->t_mpcb->ds_snd_una)
+				&& ((sent_map->flags & MAPF_IS_ACKED) == 0)
+				&& ((sent_map->flags & MAPF_IS_REXMIT) == 0)) {
+				mp_debug(DSMAP, 2, 0,"%s: inserting map %p starting at %u "
+					"for rexmit\n", __func__, sent_map,
+					(uint32_t)sent_map->ds_map_start);
+
+				// txdsmaps is from inserted at head, so higher ds
+				// values are near the start of the list. want rexmit
+				// to occur from earliest (closest to ds_una) so by
+				// inserting at head, we order from lowest to highest
+                TAILQ_INSERT_HEAD(&mp->mp_rxtmitmaps, sent_map, rxmit_map_next);
+			}
+		}
+	}
+
+	/* XXXNJW count the number of times this function is called (debugging)*/
+	mp->drexmit_count++;
+
+	MPCB_UNLOCK(mp);
+	taskqueue_enqueue(taskqueue_swi, &mp->rexmit_task);
+}
+
+/*
+ * Find the first subflow that isn't in subflow-level rexmit and do data-level
+ * rexmit on this flow (just call tcp_output, the ds_map code worries about
+ * XXXNJW: or perhaps whichever subflow has cwnd > 1MSS?
+ */
+void
+mp_rexmit_task_handler(void *context, int pending)
+{
+	struct mpcb *mp;
+	struct tcpcb *sf_tp;
+
+	mp = (struct mpcb *)context;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	// XXXNJW add checks that tp exists
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		INP_WLOCK(sf_tp->t_inpcb);
+		if (!(sf_tp->t_inpcb->inp_flags & INP_DROPPED)
+		    && sf_tp->t_rxtshift == 0
+		    && sf_tp->t_state == TCPS_ESTABLISHED) {
+			tcp_output(sf_tp);
+			INP_WUNLOCK(sf_tp->t_inpcb);
+			break;
+		}
+		INP_WUNLOCK(sf_tp->t_inpcb);
+	}
+
+	if (sf_tp == NULL) {
+		mp_debug(MPSESSION, 1, 0, "%s: all subflows in rexmit\n", __func__);
+
+//		MPCB_LOCK(mp);
+//		struct ds_map *map;
+//		TAILQ_FOREACH(map, &mp->mp_rxtmitmaps, mp_ds_map_next) {
+//			TAILQ_REMOVE(mp->mp_rxtmitmaps, map, rxmit_map_next);
+//		}
+//		MPCB_UNLOCK(mp);
+	}
+}
+
+/*
+ * Allocate PCBs to listen for incoming MP_JOINs. This is called when we
+ * advertise an address using ADD_ADDR.
+ */
+void mp_sf_alloc_task_handler(void *context, int pending) {
+        struct mpcb *mp;
+        struct socket *so;
+        struct inpcb *sf_inp;
+        struct tcpcb *sf_tp;
+        void *laddr;
+        int error;
+
+        mp = (struct mpcb *)context;
+        KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+        /* Only one subflow - no further PCBs to alloc */
+        if (mp->mp_conn_address_count == 1)
+                return;
+
+        so = mp->master_socket;
+
+        int i;
+        for (i = 1; i < mp->mp_conn_address_count; i++) {
+
+                /* Allocate an inp for the subflow */
+                INP_INFO_WLOCK(&V_tcbinfo);
+                sf_inp = in_pcballoc_subflow(so, &V_tcbinfo);
+                INP_INFO_WUNLOCK(&V_tcbinfo);
+
+                if(!sf_inp) {
+                        goto out;
+                }
+                switch (mp_usable_addresses[i].ss_family) {
+                        case AF_INET:
+                                laddr = &((struct sockaddr_in *)
+                                    &mp_usable_addresses[i])->sin_addr.s_addr;
+                                bcopy(laddr, &sf_inp->inp_laddr.s_addr, 4);
+                                break;
+                        case AF_INET6:
+                                laddr = &((struct sockaddr_in6 *)
+                                    &mp_usable_addresses[i])->sin6_addr.__u6_addr;
+                                bcopy(laddr, &sf_inp->in6p_laddr, 16);
+                                break;
+                }
+
+                /* Using same local and foreign ports as master subflow */
+                sf_inp->inp_lport = mp->master_inpcb->inp_lport;
+                sf_inp->inp_fport = mp->master_inpcb->inp_fport;
+
+                /* Foreign address is that of the master subflow */
+                sf_inp->inp_faddr.s_addr = mp->master_inpcb->inp_faddr.s_addr;
+
+                /* debug output for addresses */
+                char buf[128];
+                inet_ntop(AF_INET, &sf_inp->inp_laddr, buf, sizeof(buf));
+                mp_debug(MPSESSION, 2, 0, "%s: creating inp for %s port %d to ",
+                    __func__, buf, ntohs(sf_inp->inp_lport));
+                inet_ntop(AF_INET, &sf_inp->inp_faddr, buf, sizeof(buf));
+                mp_debug(MPSESSION, 2, 0, "%s port %d\n", buf,
+                    ntohs(sf_inp->inp_fport));
+
+                error = tcp_subflow_attach(so, sf_inp, mp);
+                if (error) {
+                        goto free;
+                }
+
+                INP_HASH_WLOCK(&V_tcbinfo);
+                in_pcbinshash(sf_inp);
+                INP_HASH_WUNLOCK(&V_tcbinfo);
+
+                sf_tp = intotcpcb(sf_inp);
+                sf_tp->t_state = TCPS_LISTEN;
+
+                /* Increase subflow count - this subflow will require a
+                 * correct shutdown, now that it is alloc'd */
+                atomic_add_int(&sf_tp->t_mpcb->subflow_cnt,1);
+                mp_debug(MPSESSION, 4, 0, "%s:%d inc sf count, tp %p\n",
+                    __func__, __LINE__, sf_tp);
+
+                INP_WUNLOCK(sf_inp);
+                break;
+        }
+
+        goto out;
+
+free:
+        in_pcbfree(sf_inp);
+out:
+        return;
+}
+
+/*
+ * Pass in mp control block in context
+ * pending == not sure, read the man page
+ *
+ * -tcp_subflow_attach(so,mp)
+ * 	 -> inpcb_alloc_subflow
+ * 	 -> tcp_newtcpcb
+ * - create sockaddr
+ * - tcp_do_subflow_connect(struct tcpcb *tp, struct sockaddr *nam,
+ *   struct thread *td)
+ */
+
+void mp_join_task_handler(void *context, int pending) {
+	struct inpcb *sf_inp;
+	struct mpcb *mp;
+	struct socket *so;
+	struct tcpcb *tp;
+	struct sockaddr dst_nam;
+	void *laddr;
+	void *faddr;
+	int ret, error;
+
+	mp = (struct mpcb *)context;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	/*
+	 * MAX_SUBFLOWS is not always the same as the
+	 * number of available addresses
+	 */
+	if(!mp->mp_advjoin_mask || mp->subflow_cnt == MAX_SUBFLOWS)
+		return;
+
+	so = mp->master_inpcb->inp_socket;
+
+	/*
+	 * Find the next address to send a JOIN for, then
+	 * assign this to addr and use when connecting the new
+	 * subflow
+	 */
+	int i;
+	for (i = 0; i < mp->mp_added_address_count; i++) {
+		/* continue if this index has sent join already */
+		if (!(mp->mp_advjoin_mask & (1 << i)))
+			continue;
+
+		/* Allocate an inp for the subflow */
+		INP_INFO_WLOCK(&V_tcbinfo);
+		sf_inp = in_pcballoc_subflow(so, &V_tcbinfo);
+		INP_INFO_WUNLOCK(&V_tcbinfo);
+
+		if(!sf_inp)
+			goto out;
+
+		mp_debug(MPSESSION, 3, 0, "%s: added address count %d ", __func__,
+		    mp->mp_added_address_count);
+		mp_debug(MPSESSION, 3, 0, "address count %d\n",
+		    mp->mp_conn_address_count);
+
+		/* if more than one available address, start MP_JOINs from the
+		 * first added address.
+		 * XXXNJW: Temporary until the path manager is implemented
+		 */
+		int j = 0;
+		switch (mp_usable_addresses[j].ss_family) {
+			case AF_INET:
+				laddr = &((struct sockaddr_in *)
+					&mp_usable_addresses[j])->sin_addr.s_addr;
+				bcopy(laddr, &sf_inp->inp_laddr, 4);
+				break;
+			case AF_INET6:
+				laddr = &((struct sockaddr_in6 *)
+					&mp_usable_addresses[j])->sin6_addr.__u6_addr;
+				bcopy(laddr, &sf_inp->in6p_laddr, 16);
+				break;
+		}
+
+		if(!mp_is_if_up((struct sockaddr *) &mp_usable_addresses[j])) {
+			mp_debug(MPSESSION, 1, 0, "%s: usable address is down\n", __func__);
+			continue;
+		}
+
+		/* Use the same local port as the master tp */
+		sf_inp->inp_lport = mp->master_inpcb->inp_lport;
+
+		/* create a tp for the subflow inp */
+		ret = tcp_subflow_attach(so, sf_inp, mp);
+
+		if(mp->master_inpcb->inp_vflag & INP_IPV4) {
+			faddr = &((struct sockaddr_in *)
+				&mp->mp_added_addresses[i])->sin_addr.s_addr;
+			bcopy(faddr, &(((struct sockaddr_in *) &dst_nam)->sin_addr.s_addr),
+				sizeof(struct in_addr));
+			((struct sockaddr_in *) &dst_nam)->sin_port
+				= mp->master_inpcb->inp_fport;
+			dst_nam.sa_len = sizeof(struct sockaddr_in);
+			dst_nam.sa_family = AF_INET;
+		} else if (mp->master_inpcb->inp_vflag & INP_IPV6) {
+			panic("ipv6 not supported yet\n");
+		}
+
+		/* ret > 1 if some kind of error */
+		if (ret) {
+			goto free;
+		}
+
+		tp = intotcpcb(sf_inp);
+		tp->t_inpcb = sf_inp;
+
+		/* debug output for addresses */
+		char buf[128];
+		inet_ntop(AF_INET, &sf_inp->inp_laddr, buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "%s: Connecting to %s port %d to ",
+			__func__, buf, ntohs(sf_inp->inp_lport));
+		inet_ntop(AF_INET, &(((struct sockaddr_in *) &dst_nam)->sin_addr),
+			buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "%s port %d\n", buf,
+			ntohs(((struct sockaddr_in *) &dst_nam)->sin_port));
+
+		mp_debug(MPSESSION, 1, 0, "%s: Connecting to %s, tp %p inp %p\n",
+			__func__, buf, tp, sf_inp);
+
+		/* Start connection and send the SYN */
+		/*
+		 * XXXNJW: passing the wrong cred here (swi cred rather than master
+		 * socket cred) Should be passing something like
+		 * 'mp->master_socket->so_cred;'
+		 */
+		error = (*so->so_proto->pr_usrreqs->pru_connect)
+			(so, &dst_nam, curthread);
+		if (error)
+			mp_debug(MPSESSION, 4, 0, "%s: pru_connect error %d\n",
+				__func__, error);
+		else {
+			mp_debug(MPSESSION, 4, 0, "%s:%d inc sf count, currently %d\n",
+				__func__, __LINE__, tp->t_mpcb->subflow_cnt);
+			atomic_add_int(&tp->t_mpcb->subflow_cnt,1);
+		}
+
+		mp->mp_advjoin_mask &= ~(1 << i);
+
+	}
+
+	goto out;
+
+free:
+	in_pcbfree(sf_inp);
+out:
+	return;
+}
+
+/*
+ * Move ds_snd_una forward and schedule drop of bytes acked. if ack'ing
+ * past snd_max, then this is an ACK of a DFIN, so we can begin to close
+ * all the subflows, and the connection.
+ *
+ * If the send window is partially ACKd, then we reset the data-level RTO
+ * timer. If the window is fully ACKd, the timer is stopped.
+ *
+ */
+void
+mp_data_ack(struct mpcb *mp, uint64_t data_ack_num) {
+
+	MPCB_LOCK(mp);
+
+	mp_debug(MPSESSION, 4, 0, "processing DACK %ju (%u), ds-una is %ju\n",
+			data_ack_num, (uint32_t)data_ack_num, mp->ds_snd_una);
+
+	if (data_ack_num > mp->ds_snd_una) {
+		/* Adjust accounting on send buffer. sb_actual is adjusted
+		 * when a 'ds-map' worth of data is acknowledged. */
+		atomic_subtract_acq_int(&mp->master_socket->so_snd.sb_cc,
+		    data_ack_num - mp->ds_snd_una);
+		mp->ds_snd_una = data_ack_num;
+	} else if (data_ack_num > mp->ds_snd_max
+	      && ((mp->mp_state == MPS_M_FIN_WAIT_1)
+	      || (mp->mp_state == MPS_M_LAST_ACK))) {
+		// issue FINs on all the subflows
+	} else if (DSEQ_LEQ(data_ack_num,mp->ds_snd_una)) {
+		/*
+		 * These could be out-of-order DACKs. seems to happen frequently
+		 * when sending from multiple subflows.
+		 */
+	}
+
+	/* If in the process of closing a socket, then we do not want to attempt
+	 * to drop any data from the buffer (it has been flushed at this point) */
+	if ((mp->master_socket->so_state & SS_ISDISCONNECTING) ||
+			(mp->master_socket->so_snd.sb_state & SBS_CANTSENDMORE)) {
+		mp_debug(MPSESSION, 1, 0,
+		    "%s: socket is flushed, updated mp accounting only\n", __func__);
+		goto no_sb_drop;
+	}
+
+	/*
+	 * In theory it would be nice to only call this inside the above if-block
+	 * when a DACK bumps ds_snd_una. Doing so unfortunately leads to
+	 * reproducible connection stalls, and the current hypothesis for why is
+	 * below:
+	 *
+	 * In the multi-subflow scenario a DACK can come in on any subflow at any
+	 * time and some ds_maps may not be able to be freed at the time ds_snd_una
+	 * is bumped due to them not being marked as ACKED by their subflow yet.
+	 * We may then get an old DACK on a subflow which we need to use as the
+	 * trigger to go back and remove maps which couldn't be deleted previously.
+	 */
+	mp_deferred_sbdrop(mp);
+
+no_sb_drop:
+	MPCB_UNLOCK(mp);
+}
+
+void
+mp_init_established(struct tcpcb *tp) {
+
+	struct mpcb *mp = tp->t_mpcb;
+	/* Enabled multipath features */
+	tp->mp_active = 1;
+
+	mp_debug(MPSESSION, 1, 0, "%s: new mp connection for tp: %p: \n",
+	    __func__, tp);
+	mp_debug(MPSESSION, 1, 0, "%s: subflow count: %d\n", __func__,
+	    mp->subflow_cnt);
+
+	/* we know the other end is mp enabled */
+	if (IS_MASTER_TP(tp)) {
+		mp->mp_state = MPS_ESTABLISHED;
+		tp->infinite_map = 0;
+		mp->ds_map_max = mp->ds_snd_una =
+			mp->ds_snd_max =mp->ds_map_min =
+			mp->ds_idsn + 1;
+		mp->ds_rcv_nxt = mp->ds_idsr + 1;
+
+		/*
+		 * Add the master address as a usable address, we may need it as a
+		 * source address if the remote host advertises addresses to us and
+		 * we need to send an MP_JOIN
+		 */
+		void *saddr = &((struct sockaddr_in *)
+		    &mp_usable_addresses[0])->sin_addr.s_addr;
+		bcopy(&mp->master_inpcb->inp_laddr, saddr, sizeof(struct in_addr));
+		mp_usable_addresses[0].ss_family = AF_INET;
+		mp_usable_addresses[0].ss_len = sizeof(struct sockaddr_in);
+
+		/*
+		 * Don't want to advertise the 'primary' address so mask out now
+		 */
+		tp->t_mpcb->mp_advaddr_mask &= ~1;
+
+		mp_debug(MPSESSION, 1, 0, "%s: advadd %u, address count %d \n",
+			__func__, tp->t_mpcb->mp_advaddr_mask, mp->mp_conn_address_count);
+
+		mp_debug(MPSESSION, 1, 0, "%s: master tp %p transitioning to "
+		    "MPS_ESTABLISHED\n", __func__, tp);
+
+		mp_debug(MPSESSION, 1, 0, "%s: idsn - %ju\n",
+		    __func__, (uintmax_t) tp->t_mpcb->ds_idsn);
+
+		mp_debug(MPSESSION, 1, 0, "%s: local token: %u, remote token: %u\n",
+			__func__, (uint32_t) tp->t_mpcb->local_token,
+			(uint32_t) tp->t_mpcb->remote_token);
+
+		mp_debug(MPSESSION, 1, 0, "%s: local key: %ju, remote key: %ju\n",
+			__func__, (uintmax_t) tp->t_mpcb->local_key,
+			(uintmax_t) tp->t_mpcb->remote_key);
+
+		mp_debug(MPSESSION, 5, 0, "%s: ds_rcv_nxt: %u\n", __func__,
+			(uint32_t) tp->t_mpcb->ds_rcv_nxt);
+
+		mp_debug(MPSESSION, 5, 0, "%s: ds_snd_una: %u\n", __func__,
+					(uint32_t) tp->t_mpcb->ds_snd_una);
+
+	}
+
+}
+
+/*
+ * Remove and free maps that have been duplicated (due to re-transmission) and
+ * have been acked at the subflow level. The map is identified as being a
+ * duplicate as soon as the first map of the same
+ */
+static void inline
+mp_drop_dup_maps(struct mpcb *mp) {
+
+	struct ds_map *map, *next_map;
+
+	map = TAILQ_FIRST(&mp->mp_ds_sndmaps);
+	while (map != NULL) {
+		next_map = TAILQ_NEXT(map, mp_ds_map_next);
+		if((map->flags & (MAPF_IS_ACKED)) &&
+				(map->flags & (MAPF_IS_DUP))) {
+			/* Only need to check ACKED/DUP as a map can only be
+			 * marked as MAPF_IS_DUP if another identical map has been ACKED
+			 * at subflow level and is below ds_snd_una. remove it from the
+			 * snd_maps list and dealloc the map. The sent bytes will be
+			 * dropped when we drop the ds_map that was acked first.
+			 *
+			 * XXXNJW: Will need to wait until the next D-ACK to finally drop
+			 * the acked bytes from sendbuffer. Would be good to restart the
+			 * process of inserting maps into 'maps to free' after removing
+			 * the duplicate.
+			 */
+			mp_debug(DSMAP, 5, 0, "%s: mark map dsn: %u as dup and drop %p\n",
+			    __func__, (uint32_t)map->ds_map_start, map);
+			TAILQ_REMOVE(&mp->mp_ds_sndmaps, map, mp_ds_map_next);
+			free(map, M_DSSMAP);
+		}
+		map = next_map;
+	}
+}
+
+/*
+ * See what ds_maps can be dropped.
+ */
+void
+mp_deferred_sbdrop(struct mpcb *mp) {
+	struct ds_map *map, *next_map;
+	struct mbuf *cannotfreembptr;
+	int ndrop = 0;
+	uint64_t ds_high_drop, ds_map_end;
+
+	/* Keep a list of maps that should be freed only if ndrop is > 0 */
+	struct dsmapq_head maps_to_free;
+	/* Keep a list of maps that are removed from ds_snd_maps if they are a
+	 * duplicate of a map that has been acked, and ndrop > 0 */
+	struct dsmapq_head dup_maps;
+
+	if (!mp->master_tcpcb->infinite_map)
+		MPCB_LOCK_ASSERT(mp); /* Called from mp_data_ack() */
+
+	mp_drop_dup_maps(mp);
+
+	TAILQ_INIT(&maps_to_free);
+	TAILQ_INIT(&dup_maps);
+	cannotfreembptr = NULL;
+	ds_high_drop = 0;
+
+	TAILQ_FOREACH(map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+		ds_map_end = map->ds_map_start + map->ds_map_len;
+
+		mp_debug(DSMAP, 5, 0, "%s: check map %p, ds_map_start: %ju, ds_map_end %ju, ds_snd_una %ju, "
+				"ds_high_drop: %ju, mbuf_start: %p, mbuf_offset: %u, mbuf_len: %d\n",
+				__func__, map, (uintmax_t) map->ds_map_start, (uintmax_t) ds_map_end,
+				(uintmax_t) mp->ds_snd_una, (uintmax_t) ds_high_drop, map->mbuf_start,
+				map->mbuf_offset, map->mbuf_start->m_len);
+
+		mp_debug(DSMAP, 5, 0, "MAPF_IS_ACKED: %u \n",
+		    (map->flags & MAPF_IS_ACKED));
+
+		/*
+		 * Can we drop this map?
+		 * - Has it been cumulatively ACKED and not a duplicate
+		 * - Does the sequence space start and end before ds_snd_una
+		 */
+		if ((map->flags & MAPF_IS_ACKED) && map->ds_map_start < mp->ds_snd_una
+		    && ds_map_end <= mp->ds_snd_una &&
+		    ((map->flags & MAPF_IS_DUP) == 0)) {
+
+			if (ds_high_drop == 0)
+				ndrop = map->ds_map_len;
+			else if (ds_high_drop < ds_map_end)
+				ndrop += ds_map_end - ds_high_drop;
+
+			ds_high_drop = ulmax(ds_map_end, ds_high_drop);
+
+			mp_debug(DSMAP, 5, 0, "%s: ds_high_drop: %ju, ndrop: %d, sb_cc: %d\n",
+			    __func__, (uintmax_t)ds_high_drop, ndrop,
+				mp->master_socket->so_snd.sb_cc);
+
+			/*
+			 * We are re-using the sf_ds_map_next link as it is no longer
+			 * used at the subflow level (the map is removed from the subflow
+			 * map list before it is marked as MAPF_IS_ACKED).
+			 */
+			TAILQ_INSERT_HEAD(&maps_to_free, map, sf_ds_map_next);
+
+		} else if (map->ds_map_start >= mp->ds_snd_una
+		      || map->ds_map_start > ds_high_drop) {
+
+			/* CHECK mbuf_start + map->mbuf_offset against other maps to be freed
+			 * and only allow free if they won't invalidate other mbuf_offset values.
+			 */
+			cannotfreembptr = map->mbuf_start;
+			mp_debug(DSMAP, 5, 0, "%s: reached end of freeable maps at mbuf %p len %d\n\n",
+			    __func__, cannotfreembptr, cannotfreembptr->m_len);
+			/* The list is ordered based on ascending ds_map_start, therefore maps
+			 * including and after this map cannot be freed. OR This map does not
+			 * overlap with any previous maps therefore it is safe to free up to
+			 * but not including this map. */
+			break;
+		} else {
+			/* We have overlap between a completed and uncompleted map that
+			 * is below una or we have a map that overlaps and extends past
+			 * una */
+			mp_debug(DSMAP, 5, 0, "%s: map overlap below una, can't drop anything\n", __func__);
+
+			struct ds_map *acked_map;
+			/* check if this map completely overlaps with a map that is ACKED
+			 */
+			TAILQ_FOREACH(acked_map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+				/* A duplicate map will have the same start and end ds_num,
+				 * but will not have been ACKED */
+				if ((map->ds_map_start == acked_map->ds_map_start) &&
+				    (map->ds_map_len == acked_map->ds_map_len) &&
+				    (map != acked_map) && (acked_map->flags & MAPF_IS_ACKED)) {
+					    mp_debug(DSMAP, 5, 0, "%s: found dup %p of acked map "
+					    	"%p ds_start %u acked map mbuf start %p\n",
+						    __func__, map, acked_map,
+						    (uint32_t) map->ds_map_start,
+						    acked_map->mbuf_start);
+						map->flags |= MAPF_IS_DUP;
+
+//						/* Might have multiple dups. We do not want to add them
+//						 * to the dup_list twice so need to check the dup_maps
+//						 * list for this map */
+//						TAILQ_FOREACH(dup_map, &dup_maps, mp_dup_map_next) {
+//							if (map == dup_map)
+//								dup_is_in_list = 1;
+//						}
+//
+//						/* Use a new link here - mp_dup_map_next - as the mp_
+//						 * and sf_ links are being used (sf_ds_map_next link
+//						 * is used in the "maps to free" list).
+//						 */
+//						if (!dup_is_in_list)
+//							TAILQ_INSERT_HEAD(&dup_maps, map, mp_dup_map_next);
+				}
+			}
+
+			/* found a duplicate. continue looking for maps to free as there
+			 * may be more maps following the duplicate */
+//			if (found_dup)
+//				continue;
+
+			/* Often in here due to DACKs arriving on a different subflow to
+			 * what the map was sent on. hence the map may not have been acked
+			 * at the subflow level and not marked as "MAPF_IS_ACKED". the map
+			 * will not be freed until this flag has been set */
+
+			ndrop = 0;
+			break;
+		}
+	}
+
+	/* Error as we are dropping more than in the sb */
+	KASSERT(ndrop <= mp->master_socket->so_snd.sb_actual,
+	    ("%s: ndrop %d > sb_actual %d\n" , __func__, ndrop,
+	    mp->master_socket->so_snd.sb_actual));
+
+
+	if (ndrop > 0) {
+		int bytes_from_end;		   // sqn in mbuf_end when the map ends
+		struct mbuf *map_mbuf_end; // The mbuf in which this map ends
+		struct mbuf *last_dropped;
+
+//		/* get rid of dup maps at this time. We know that the acked map will
+//		 * be dropped */
+//		struct ds_map *dup_map;
+//		TAILQ_FOREACH(dup_map, &dup_maps, mp_dup_map_next) {
+//			/* Will check MAPF_IS_DUP in tcp_ackmap. if found in tcp_ackmap,
+//			 * the map will be freed there as the data-level portion has
+//			 * been acked and the data freed from the send buffer */
+//			printf("%s: removing dup map %p from mp_sndmaps, set dup flag\n",
+//			    __func__, dup_map);
+//			dup_map->flags |= MAPF_IS_DUP;
+//
+//			/* use the mp_ds_map_next link so that mp_ds_sndmaps is updated
+//			 * correctly. dup_maps is INITd each time we come into deferred_
+//			 * sb_drop so dot not need to remove from the dup list */
+//			TAILQ_REMOVE(&mp->mp_ds_sndmaps, dup_map, mp_ds_map_next);
+//		}
+
+		map = TAILQ_FIRST(&maps_to_free);
+		while (map != NULL) {
+			next_map = TAILQ_NEXT(map, sf_ds_map_next);
+
+			map_mbuf_end = map->mbuf_start;
+			bytes_from_end = map->ds_map_len + map->mbuf_offset;
+
+//			/* this is a small map that ends within the length of the mbuf, and
+//			 * we have another map that might occupy this same mbuf */
+//			if (bytes_from_end < map_mbuf_end->m_len
+//			    && next_map && next_map->mbuf_start == map->mbuf_start) {
+//				printf("%s: bytes_from_end %d, mb_len %d, map_start %u\n",
+//				    __func__, bytes_from_end, map_mbuf_end->m_len,
+//				    (uint32_t) map->ds_map_start);
+//				TAILQ_REMOVE(&mp->mp_ds_sndmaps, map, mp_ds_map_next);
+//				map = next_map;
+//				continue;
+//			}
+
+			while (bytes_from_end > map_mbuf_end->m_len) {
+				bytes_from_end -= map_mbuf_end->m_len;
+				map_mbuf_end = map_mbuf_end->m_next;
+			}
+
+			if (map_mbuf_end == cannotfreembptr) {
+				mp_debug(DSMAP, 4, 0, "%s: snd_dsmap %p TWEAKED - "
+						"bytes_from_end: %d\n", __func__, map, bytes_from_end);
+				mp_debug(DSMAP, 4, 0, "    PRE TWEAK - ndrop: %d, "
+				    "ds_map_start: %ju ds_map_len: %u, mbuf_start: %p, "
+				    "mbuf_offset: %u, mbuf_len: %d ds_high_drop: %ju\n",
+					ndrop, (uintmax_t)map->ds_map_start, map->ds_map_len,
+					map->mbuf_start, map->mbuf_offset, map->mbuf_start->m_len,
+					(uintmax_t)ds_high_drop);
+
+				if(map->mbuf_start != map_mbuf_end) {
+					map->mbuf_start = map_mbuf_end;
+					map->mbuf_offset = 0;
+					map->ds_map_start = map->ds_map_start +
+					    map->ds_map_len - bytes_from_end;
+					map->ds_map_len = bytes_from_end;
+				}
+
+				if (ds_high_drop > map->ds_map_start) {
+					ndrop -= ds_high_drop - map->ds_map_start;
+					KASSERT(ndrop >= 0, ("%s: negative ndrop value - "
+					    "%d\n", __func__, ndrop));
+					ds_high_drop = map->ds_map_start;
+				}
+
+				mp_debug(DSMAP, 5, 0, "    POST TWEAK - ndrop: %d, "
+				    "ds_map_start: %ju ds_map_len: %u, mbuf_start: %p, "
+				    "mbuf_offset: %u, mbuf_len: %d, ds_high_drop: %ju\n\n",
+					ndrop, (uintmax_t)map->ds_map_start, map->ds_map_len,
+					map->mbuf_start, map->mbuf_offset, map->mbuf_start->m_len,
+					(uintmax_t)ds_high_drop);
+
+			} else {
+				/* Map has yet to be removed from the mp snd list, so we do so
+				 * here before freeing the the map from memory, to prevent
+				 * dangling pointers in the mp snd list. The map is no longer
+				 * needed from this point. */
+				TAILQ_REMOVE(&mp->mp_ds_sndmaps, map, mp_ds_map_next);
+				last_dropped = map->mbuf_start;
+				mp_debug(DSMAP, 5, 0, "%s: snd_dsmap %p DELETED. map_len: "
+				    "%d, mbuf_start: %p, sf_seq_start: %u\n",
+				    __func__, map, map->ds_map_len, map->mbuf_start,
+				    map->sf_seq_start);
+				free(map, M_DSSMAP);
+			}
+			map = next_map;
+	     }
+
+		/*
+		 * (1) Make sure we are not dropping more than is in the send buffer
+		 * (2) Make sure that we are not dropping more than has been accounted
+		 *     for. (i.e. dont drop more than una/sb_actual - should not drop
+		 *     below the length of sb_cc
+		 */
+		KASSERT((int)mp->master_socket->so_snd.sb_actual - ndrop >= 0
+				&& mp->master_socket->so_snd.sb_cc <= mp->master_socket->so_snd.sb_actual - ndrop,
+			("%s: SB_DEFERACCT sb_cc/sb_actual accounting error: "
+			"sb_cc %u, sb_actual %u, len %d", __func__, mp->master_socket->so_snd.sb_cc,
+			mp->master_socket->so_snd.sb_actual, ndrop));
+
+	     mp_debug(DSMAP, 4, 0, "%s: Dropping %d bytes from socket "
+	         "buffer\n", __func__, ndrop);
+	 	 SOCKBUF_LOCK(&mp->master_socket->so_snd);
+	     sbdrop_locked(&mp->master_socket->so_snd, ndrop);
+	     mp->ds_map_min += ndrop;
+	     sowwakeup_locked(mp->master_socket);
+	} else if ((mp->ds_snd_una == mp->ds_snd_max)
+	    && (mp->master_socket->so_snd.sb_cc == 0)) {
+		/* We might not not be able to drop segments due to duplicate maps.
+		 * However is we have sent a full window and sb_cc is zero, then
+		 * everything has been acked and we should try to continue sending
+		 * new data. This call sowwakeup on the so_snd */
+//		printf("%s: attempt sowwakeup ds_una %u actual %d\n",
+//		    __func__, (uint32_t) mp->ds_snd_una,
+//		    mp->master_socket->so_snd.sb_actual);
+//		printf("%s: sbspace %ld, tp %p\n", __func__,
+//		    sbspace(&mp->master_socket->so_snd),
+//		    mp->master_tcpcb);
+		sowwakeup(mp->master_socket);
+	}
+
+}
+
+/*
+ * Accesses socket buffer on behalf on a subflow and returns a mapping
+ * into the send buffer than the subflow will transmit from.
+ *
+ * XXXNJW: currently restricted to returning a map of size 1420 (this allows
+ * space for header and options to be added while staying under the MSS)
+ */
+struct ds_map*
+mp_get_map(struct mpcb *mp, unsigned int sf_mss) {
+	struct ds_map *map = NULL;
+	struct socket *so;
+	int map_length;
+
+	MPCB_LOCK(mp);
+	so = mp->master_socket;
+	SOCKBUF_LOCK(&so->so_snd);
+
+	/*
+	 * XXXNJW do we have maps to be rexmitted?
+	 * if yes, find if this map has an existing dup in sndmaps marked as
+	 * MAPF_IS_REXMIT. if this is the case, the map will be a retransmit of
+	 * a retransmit. continue to the next map (not sure what to do with
+	 * retransmits of retransmits at the data-level).
+	 */
+	TAILQ_FOREACH(map, &mp->mp_rxtmitmaps, mp_ds_map_next) {
+		struct ds_map *next_map;
+		int skip_map = 0;
+
+		/*
+		 * Is a dup marked as MAPF_IS_REXMIT? then it is a rexmitted map
+		 * that hasn't been acked. how to handle this situation? the tp is
+		 * important, as we don't want to remap sf_sqns. rexmitted maps may
+		 * end up in this list if enough TCP-level RTOs take place _after_
+		 * it has been previously transmitted (and as a rexmit of another
+		 * segment). These maps can't be re-mapped (within the same sf)
+		 * as it will cause the ds and sf sequence spaces to become out of
+		 * sync.
+		 *
+		 * Note: The function enqueuing segments for rexmit will ignore maps
+		 * that are marked as "MAPF_IS_REXMIT", so these checks are redundant
+		 * for the current behaviour.
+		 */
+		TAILQ_FOREACH(next_map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+			if ((next_map->ds_map_start == map->ds_map_start)
+			    && (next_map->flags & MAPF_IS_REXMIT)
+			    && next_map != map) {
+				skip_map = 1;
+				break;
+			}
+		}
+
+		if (skip_map) {
+			mp_debug(DSMAP, 2, 0, "%s: found existing rexmit %p\n", __func__,
+			    map);
+			continue;
+		}
+
+		// for now not rexmitting rexmits.... (if the RTOs of two subflows
+		// can't deal with it, in the current implementation that would be
+		// a stalled connection.
+		if (map->flags & MAPF_IS_REXMIT)
+			continue;
+
+		/* The map is okay to rexmit */
+		break;
+	}
+
+	if(map) {
+		// set a flag that will cause mp_get_map to return the map nearest
+		// ds_snd_una so that it can be retransmitted.
+		mp->doing_rxmit = 1;
+
+		struct ds_map *rxmit_map;
+		rxmit_map = malloc(sizeof(struct ds_map), M_DSSMAP, M_NOWAIT|M_ZERO);
+		/* copy the data-level info to the new map
+		 * subflow details are added on return to tcp_output */
+		rxmit_map->mbuf_start = map->mbuf_start;
+		rxmit_map->mbuf_offset = map->mbuf_offset;
+		rxmit_map->ds_map_start = map->ds_map_start;
+		rxmit_map->ds_map_len = map->ds_map_len;
+		rxmit_map->flags |= MAPF_IS_REXMIT;
+		mp_insert_map(mp, rxmit_map);
+
+		mp_debug(DSMAP, 2, 0, "%s: attempt to rexmit map %p dsn: %u tseq: %u\n",
+			__func__, map, (uint32_t) map->ds_map_start, map->sf_seq_start);
+
+		/*
+		 * remove the map from the "to retransmit" list. If this retransmit
+		 * fails then it may be inserted into the rxtmitmaps list itself.
+		 *
+		 * XXXNJW: Should the rexmit be retransmitted, or again copy the
+		 * original and try a different subflow?
+		 */
+		TAILQ_REMOVE(&mp->mp_rxtmitmaps, map, rxmit_map_next);
+
+		SOCKBUF_UNLOCK(&so->so_snd);
+		MPCB_UNLOCK(mp);
+
+		return(rxmit_map);
+
+	}
+
+	if (mp->doing_rxmit)
+		mp_debug(REASS, 3, 0, "%s: exiting need rexmit\n", __func__);
+
+	mp->doing_rxmit = 0;
+
+	/* If there are bytes in the send buff that could potentially be
+	 * sent
+	 * */
+	if(so->so_snd.sb_cc) {
+		/* XXXNJW: the single map per packet workaround is horrible but works
+		 * well enough for debugging. 1420 is a safe enough size that we know
+		 * the segment wont be split into multiple packets or trigger sendalot
+		 */
+		if (single_packet_maps)
+			map_length = min(SB_NUNMAPPED(&so->so_snd, mp), 1420);
+		else {
+			if (so->so_snd.sb_cc >= so->so_snd.sb_hiwat
+				&& (SB_NUNMAPPED(&so->so_snd, mp) > sf_mss))
+				map_length = SB_NUNMAPPED(&so->so_snd, mp) / mp->subflow_cnt;
+			else
+				map_length = SB_NUNMAPPED(&so->so_snd, mp);
+		}
+
+		mp_debug(DSMAP, 3, 0, "%s - sbcc: %u, sb_hiwat: %u, unmapped:_cc %d, "
+			"subflow_cnt: %d\n",
+		    __func__, so->so_snd.sb_cc, so->so_snd.sb_hiwat,
+		    SB_NUNMAPPED(&so->so_snd, mp), mp->subflow_cnt);
+
+		KASSERT (so->so_snd.sb_actual >= (mp->ds_map_max - mp->ds_map_min),
+			("%s: Negative map length\n", __func__));
+
+		if (map_length > 0) {
+			map = malloc(sizeof(struct ds_map), M_DSSMAP, M_NOWAIT|M_ZERO);
+
+			if (map) {
+				/* XXXNJW: in future at this point want to decide how
+				 * many bytes are sent from the sockbuf rather than simply
+				 * sbchars.
+				 */
+				map->mbuf_start = sbsndptr(&so->so_snd,
+					(mp->ds_map_max - mp->ds_map_min),
+					map_length, &map->mbuf_offset);
+
+				map->ds_map_start = mp->ds_map_max;
+				map->ds_map_len = map_length;
+
+				mp_debug(DSMAP, 4, 0, "%s: map %p, ds_map_start %ju, "
+					"ds_map_end %ju, mbuf_start %p, mb_offset: %u, offset: "
+					"%ju, maplen: %d, sbcc: %u, sb_mb: %p\n",
+				    __func__, map, (uintmax_t)map->ds_map_start,
+				    (uintmax_t)map->ds_map_start+map_length, map->mbuf_start,
+				    map->mbuf_offset, (uintmax_t)(mp->ds_map_max - mp->ds_snd_una),
+					map_length, so->so_snd.sb_cc,so->so_snd.sb_mb);
+				mp->ds_map_max += map_length;
+				mp_insert_map(mp, map);
+			}
+		}
+	}
+
+	SOCKBUF_UNLOCK(&so->so_snd);
+	MPCB_UNLOCK(mp);
+
+	return(map);
+}
+
+void
+mp_insert_map(struct mpcb *mp, struct ds_map *map) {
+
+	// do ordered insert of map into list by ds_start value
+	/*
+	 * Insert the map in data-sequence order by finding an existing
+	 * map whose ds_start value is greater than or equal to the new
+	 * map. Insert the new map before this. If this map is not before
+	 * any existing maps, insert at the tail.
+	 */
+	struct ds_map *next_map;
+	TAILQ_FOREACH(next_map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+		if (next_map->ds_map_start == map->ds_map_start) {
+			mp_debug(DSMAP, 4, 0, "%s: inserting %p, dup of %p ds: %u\n",
+			    __func__, map, next_map, (uint32_t) map->ds_map_start);
+		}
+
+		if (next_map->ds_map_start >= map->ds_map_start) {
+			TAILQ_INSERT_BEFORE(next_map, map, mp_ds_map_next);
+			mp_debug(DSMAP, 4, 0, "inserted map %p before map %p\n",
+				map, next_map);
+			break;
+		}
+	}
+	if (next_map == NULL) {
+		mp_debug(DSMAP, 4, 0, "inserting map %p at tail of "
+			"mp_ds_sndmaps, is mp_ds_sndmaps empty %d\n",
+			map, TAILQ_EMPTY(&mp->mp_ds_sndmaps));
+		TAILQ_INSERT_TAIL(&mp->mp_ds_sndmaps, map, mp_ds_map_next);
+	}
+
+	/* XXX: Can go under #ifdef INVARIANTS */
+	int total_mapped = 0;
+	uint64_t last_map_end = 0;
+	uint64_t ds_high_drop = 0, ds_map_end = 0;
+	TAILQ_FOREACH(next_map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+		ds_map_end = next_map->ds_map_start + next_map->ds_map_len;
+
+		/* If this map doesn't progress any futher towards snd_max, then
+		 * we continue past it. */
+		if ((last_map_end > 0) && (ds_map_end == last_map_end))
+			continue;
+
+		if (ds_high_drop == 0)
+			total_mapped = next_map->ds_map_len;
+		else if (ds_high_drop < ds_map_end)
+			total_mapped += ds_map_end - ds_high_drop;
+
+		ds_high_drop = ulmax(ds_map_end, ds_high_drop);
+		last_map_end = ds_map_end;
+	}
+
+	KASSERT(total_mapped == mp->ds_map_max - mp->ds_map_min,
+			("total bytes in ds maps %d not equal to mapped bytes in sockbuf "
+			"(ds_map_max - ds_map_min) %ju - %ju = %ju\n",
+			total_mapped, (uintmax_t)mp->ds_map_max, (uintmax_t)mp->ds_map_min,
+			(uintmax_t)(mp->ds_map_max - mp->ds_map_min)));
+
+}
+
+struct ds_map *
+mp_find_dsmap(struct tcpcb *tp, tcp_seq	seqnum)
+{
+	struct ds_map *map;
+	tcp_seq sf_seq_end;
+	int new_map, rexmit_map;
+	struct socket *so = tp->t_mpcb->master_socket;
+	struct mpcb *mp = tp->t_mpcb;
+
+	new_map = 0;
+	rexmit_map = 0;
+	map = NULL;
+
+	mp_debug(DSMAP, 3, 0, "%s - sbcc: %u, sb_hiwat: %u, unmapped:_cc %d, "
+		"subflow_cnt: %d\n",
+		__func__,
+		so->so_snd.sb_cc, so->so_snd.sb_hiwat,
+		SB_NUNMAPPED(&so->so_snd, mp), mp->subflow_cnt);
+
+
+	/* Match the first map sitting in txdmaps that is marked as "REXMIT" and
+	 * attempt to transmit this first. it may have been inserted previously
+	 * but not sent. The sequence numbers etc will have been already set.
+	 */
+	TAILQ_FOREACH(map, &tp->t_txdsmaps, sf_ds_map_next) {
+		if ((map->flags & MAPF_IS_REXMIT) && !(map->flags & MAPF_IS_SENT))
+			return(map);
+	}
+
+	/* Only look for maps to transmit if there aren't any already awaiting a
+	 * ds-level retransmit, and sf is not in sf-level rexmit already.
+	 */
+	if (!TAILQ_EMPTY(&mp->mp_rxtmitmaps) && (tp->t_rxtshift == 0))
+		rexmit_map = 1;
+	else {
+		/* Performing wrap detection on map lookups based on sequence number.
+		 * If (sf_seq_end = sf_start + length) wraps, sf_seq_end will be less
+		 * than sf_seq_start. We detect this with: seqnum <= sf_seq_end (seqnum
+		 * is in map) && sf_seq_end < map->sf_seq_start
+		 */
+		TAILQ_FOREACH(map, &tp->t_txdsmaps, sf_ds_map_next) {
+
+			/* reached end of the map list, */
+			sf_seq_end =  map->sf_seq_start + map->ds_map_len;
+			if (seqnum == sf_seq_end) {
+				new_map = 1;
+				break;
+			}
+
+			/* match the first map which covers the passed in sequence num.
+			 * includes checks for wrapped maps. */
+			if ((seqnum >= map->sf_seq_start && sf_seq_end > map->sf_seq_start
+				&& seqnum < sf_seq_end) ||
+				(sf_seq_end < map->sf_seq_start && seqnum < map->sf_seq_start
+				&& seqnum < sf_seq_end) ||
+				(sf_seq_end < map->sf_seq_start && seqnum >= map->sf_seq_start
+				&& seqnum <= UINT_MAX)) {
+					mp_debug(DSMAP, 4, 0, "%s  sequence num: %u"
+						"starts at %u, ends %u\n", __func__,
+						seqnum, map->sf_seq_start, sf_seq_end);
+					break;
+			}
+		}
+	}
+
+//	/* Try to force availability of data to send when we have multiple
+//	 * subflows. This causes a map to not be returned on alternate calls to
+//	 * mp_find_dsmap (except in cases where we are servicing an already
+//	 * allocated map (from the code above).
+//	 */
+//	if (tp->t_mpcb->subflow_cnt > 1 && map == NULL
+//	    && !(tp->sf_flags & SFF_DATA_WAIT) && !rexmit_map) {
+//		tp->sf_flags |= SFF_DATA_WAIT;
+//		return NULL;
+//	}
+
+	if (map == NULL || new_map || rexmit_map) {
+		if ((map == NULL) && (rexmit_map == 0))
+			KASSERT(TAILQ_EMPTY(&tp->t_txdsmaps),
+			    ("%s: %u below sf_seq_start of oldest una map",
+			    __func__, seqnum));
+
+		map = mp_get_map(tp->t_mpcb, tp->t_maxseg);
+		if (map) {
+			map->sf_seq_start = seqnum;
+			map->sf_tp = tp;
+			TAILQ_INSERT_HEAD(&tp->t_txdsmaps, map, sf_ds_map_next);
+
+			/* If this sf is retransmitting a data-segment, don't need to
+			 * change the "wait" status */
+			if ((tp->sf_flags & SFF_DATA_WAIT) && !rexmit_map) {
+				tp->sf_flags &= ~SFF_DATA_WAIT;
+
+				atomic_subtract_acq_int(&tp->t_mpcb->data_wait_count, 1);
+
+				KASSERT(tp->t_mpcb->data_wait_count >= 0,
+				    ("%s data wait count < 0\n",__func__));
+				mp_debug(DSMAP, 5, 0, "tp %p got data after waiting, length: "
+				    "%d\n", tp, map->ds_map_len);
+			}
+			/* Have at least one subflow that had to wait to send data,
+			 * enqueue to data task so that tcp_output can be called. */
+			if (atomic_load_acq_int(&tp->t_mpcb->data_wait_count) > 0) {
+				taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->data_task);
+			}
+		} else {
+			/* No map was returned. should set SFF_DATA_WAIT to ensure that
+			 * we try to allocate a map to this subflow in the next call to
+			 * mp_find_dsmap
+			 */
+			if (!(tp->sf_flags & SFF_DATA_WAIT)) {
+				tp->sf_flags |= SFF_DATA_WAIT;
+				atomic_add_acq_int(&tp->t_mpcb->data_wait_count, 1);
+			}
+		}
+	}
+
+	return (map);
+}
+
+
+
+/*
+ * When a zero window is opened up, we should let all the subflows know.
+ * Can do this by triggering an ACK (the new window will be placed in the
+ * header
+ */
+void
+mp_update_subflow_windows(struct tcpcb *tp) {
+	struct mpcb *mp = tp->t_mpcb;
+	struct tcpcb *sf_tp;
+	int	sf_count;
+
+	INP_INFO_WLOCK(&V_tcbinfo);
+	sf_count = mp->subflow_cnt - 1;
+
+	/* -1 since we skip the calling tp */
+	struct inpcb *subflow_inps[sf_count];
+
+	int i = 0;
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		if (sf_tp == tp)
+			continue;
+		INP_RLOCK(sf_tp->t_inpcb);
+		subflow_inps[i] = sf_tp->t_inpcb;
+		in_pcbref(sf_tp->t_inpcb);
+		INP_RUNLOCK(sf_tp->t_inpcb);
+		i++;
+	}
+	INP_INFO_WUNLOCK(&V_tcbinfo);
+
+
+	/* Only master subflow exists */
+	if (i == 0)
+		return;
+
+	for (i = 0; i < sf_count; i++) {
+		struct inpcb *sf_inp = subflow_inps[i];
+		INP_WLOCK(sf_inp);
+		if ((sf_inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED))) {
+			in_pcbrele(sf_inp);
+			INP_WUNLOCK(sf_inp);
+			continue;
+		}
+
+		sf_tp = intotcpcb(sf_inp);
+		sf_tp->t_flags |= TF_ACKNOW;
+		tcp_output(sf_tp);
+		if (in_pcbrele(sf_inp))
+			continue;
+		INP_WUNLOCK(sf_inp);
+	}
+
+	return;
+}
+
+void
+mp_subflow_is_disconnected(struct tcpcb *tp) {
+
+	struct mpcb *mp = tp->t_mpcb;
+	struct tcpcb *sf_tp;
+	int all_closed = 1;
+
+	/* Do not want to call soisdisconnected if master tp is still
+	 * progressing through connection teardown. */
+	if (tp->t_state < TCPS_FIN_WAIT_2)
+		return;
+
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		if (sf_tp == tp)
+			continue;
+
+		mp_debug(MPSESSION, 1, 0, "%s: tp state: %d master: %d\n", __func__,
+			sf_tp->t_state, IS_MASTER_TP(sf_tp) ? 1 : 0);
+
+		/*
+		 * The calling thread holds a write lock on the tp
+		 * that is just transitioning to FIN_WAIT_2. When this function
+		 * is run from a different context it will not race with this
+		 * thread when checking its state.
+		 */
+		INP_RLOCK(sf_tp->t_inpcb);
+
+		/* Do not call soisdisconnected while at least one subflow is
+		 * still in connection teardown */
+		if (sf_tp->t_state < TCPS_FIN_WAIT_2)
+			all_closed = 0;
+		INP_RUNLOCK(sf_tp->t_inpcb);
+		if (!all_closed)
+			break;
+	}
+
+	if(all_closed) {
+		mp_debug(MPSESSION, 1, 0, "%s: all closed\n", __func__);
+		soisdisconnected(mp->master_socket);
+	}
+
+
+}
+
+/*
+ * Used to determine when we can set the socket as "cantrcvmore". Needed
+ * as subflows will shut-down asynchronously at times.
+ * XXXNJW: To be overhauled when DFIN support is added in.
+ */
+void
+mp_set_cantrcvmore(struct tcpcb *tp) {
+	struct mpcb *mp = tp->t_mpcb;
+	struct socket *so = mp->master_socket;
+	struct tcpcb *sf_tp;
+	int all_finned = 1;
+
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		if (sf_tp == tp)
+			continue;
+
+		if (sf_tp->t_state == TCPS_ESTABLISHED ||
+			((sf_tp->t_state > TCPS_ESTABLISHED) &&
+			(sf_tp->t_state != TCPS_CLOSE_WAIT) &&
+		    (sf_tp->t_state != TCPS_TIME_WAIT) &&
+		    (sf_tp->t_state != TCPS_CLOSING))) {
+			all_finned = 0;
+			break;
+		}
+	}
+
+	if (all_finned)
+		if ((so->so_rcv.sb_state & SBS_CANTRCVMORE) == 0)
+			socantrcvmore(so);
+}
+
+/* If we are attempting to use an interface, need to make sure that it is up.
+ * this is more important for things like sending ADD_ADDRs on interfaces that
+ * aren't up (this can happen if the sysctl is configured to use an address,
+ * and that address happens to be down/non existent */
+int
+mp_is_if_up(struct sockaddr *l_addr) {
+	struct ifaddr *ifa = NULL;
+
+	ifa = ifa_ifwithaddr(l_addr);
+	if (!ifa)
+		return 0;
+
+	return (ifa->ifa_ifp->if_flags & IFF_UP);
+}
+
+/* If the added address is being used in the master_tp (i.e. is default route)
+ * we do not want to be advertising this (it will eventually try to bind the
+ * same address and port as the master tp, causing problems) */
+int
+mp_is_addr_default(struct sockaddr_in *l_addr, struct mpcb *mp) {
+	void *addr;
+	char inet_buf[64];
+
+	addr = &l_addr->sin_addr;
+	inet_ntop(AF_INET, addr, inet_buf, mp_usable_addresses[0].ss_len);
+	mp_debug(MPSESSION, 1, 0, "%s: Comparing added address %s ", __func__,
+	    inet_buf);
+
+	addr = &((struct sockaddr_in *) &mp_usable_addresses[0])->sin_addr;
+	inet_ntop(AF_INET, addr, inet_buf, mp_usable_addresses[0].ss_len);
+	mp_debug(MPSESSION, 1, 0, "with default %s\n", inet_buf);
+
+	/* is the passed-in interface the same as the default interface  */
+	if (l_addr->sin_addr.s_addr == mp->master_inpcb->inp_laddr.s_addr)  {
+		mp_debug(MPSESSION, 1, 0, "%s: mp_addresses entry same as master_tp "
+		    "addr\n", __func__);
+		return 1;
+	}
+
+	mp_debug(MPSESSION, 1, 0, "%s: mp_addresses entry not same as master_tp "
+	    "addr\n", __func__);
+	return 0;
+}
+
+void
+mp_update_available_addresses(struct mpcb *mp) {
+	struct mp_add address;
+	int new_address_count = mp->mp_conn_address_count;
+	int i;
+
+	address.length = address.sub_ipver = 0;
+
+	/* Loop through available addresses. If the interface is down then it
+	 * is not included in this connection. If the interface is the same as the
+	 * default interface of the connection, it is not included. */
+	for (i = 1; i < mp->mp_conn_address_count; i++) {
+		if (!mp_is_if_up((struct sockaddr *) &mp_usable_addresses[i]) ||
+			mp_is_addr_default((struct sockaddr_in *) &mp_usable_addresses[i],
+			mp)) {
+			mp->mp_advaddr_mask &= ~(1 << i);
+			new_address_count--;
+		}
+		mp->mp_conn_address_count = new_address_count;
+	}
+
+}
+
+
+void
+mp_process_local_key(struct mpcb *mp, uint64_t local_key)
+{
+	uint8_t digest[20];
+	char buf[256];
+
+	mp_do_sha_hash(digest, (uint8_t*) &local_key, sizeof(local_key));
+
+	mp->ds_idsn = mp_new_idsn(digest);
+	mp->local_key = local_key;
+
+	/* As linux does this, we need to do it for interop */
+	mp->ds_idsn = htobe64(mp->ds_idsn);
+	mp->local_token = mp_get_token(digest);
+
+	btohex(buf, sizeof(buf), digest, sizeof(digest), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "SHA1(local_key) = 0x%s\n", buf);
+
+	btohex(buf, sizeof(buf), (uint8_t *)&mp->ds_idsn,
+			sizeof(mp->ds_idsn), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "local idsn = 0x%s\n", buf);
+	mp_debug(MPSESSION, 4, 0, "local idsn = %u\n", (uint32_t)mp->ds_idsn);
+
+}
+void
+mp_process_remote_key(struct mpcb *mp, uint64_t remote_key)
+{
+	uint8_t digest[20];
+	char buf[256];
+
+	mp_do_sha_hash(digest, (uint8_t*) &remote_key, sizeof(remote_key));
+
+	mp->ds_idsr = mp_new_idsn(digest);
+	mp->remote_key = remote_key;
+
+	mp->ds_idsr = htobe64(mp->ds_idsr);
+	mp->remote_token = mp_get_token(digest);
+
+	mp_debug(MPSESSION, 4, 0, "%s: idsr: %ju : %u\n", __func__,
+		(uintmax_t)mp->ds_idsr, (uint32_t)mp->ds_idsr);
+
+	/* debug output from hash */
+	btohex(buf, sizeof(buf), digest, sizeof(digest), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "SHA1(remote_key) = 0x%s\n", buf);
+
+	btohex(buf, sizeof(buf), (uint8_t *)&mp->ds_idsr,
+			sizeof(mp->ds_rcv_nxt), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "remote idsn = 0x%s\n", buf);
+	mp_debug(MPSESSION, 4, 0, "remote idsn = %u\n", (uint32_t)mp->ds_idsr);
+}
+
+
+SYSCTL_VNET_PROC(_net_inet_tcp_mptcp, OID_AUTO, mp_addresses, CTLTYPE_STRING|CTLFLAG_RW,
+    NULL, 0, mp_addresses, "A", "extra addresses to be used in Multipath TCP connections");
+
+SYSCTL_VNET_PROC(_net_inet_tcp_mptcp, OID_AUTO, mp_debug, CTLTYPE_STRING|CTLFLAG_RW,
+    NULL, 0, mp_debug_sysctl_handler, "A", "Enable debugging output for mptcp");
diff -r 1e55e1aa8b7f sys/netinet/mptcp_var.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_var.h	Fri Jul 11 13:32:06 2014 +1000
@@ -0,0 +1,478 @@
+/*-
+ * Copyright (c) 2012-2013
+ * 	Swinburne University of Technology, Melbourne, Australia.
+ * All rights reserved.
+ *
+ * This software was developed at the Centre for Advanced Internet
+ * Architectures, Swinburne University of Technology, by Nigel Williams and
+ * Lawrence Stewart, made possible in part by a gift from The Cisco University
+ * Research Program Fund, a corporate advised fund of Silicon Valley Community
+ * Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * mptcp_var.h
+ *
+ *  Created on: 15/05/2012
+ *      Author: nwilliams
+ */
+
+#ifndef MPTCP_VAR_H_
+#define MPTCP_VAR_H_
+
+#define MAX_SUBFLOWS 8
+#define MAX_ADDRS 	8
+
+#define	BTOHEX_MSBLTOR	0x01
+#define	BTOHEX_MSBRTOL	0x02
+
+struct tcpopt;
+
+/*
+ * Globally accessible index of addresses that can be
+ * used for mp_enabled connections. set via a sysctl.
+ * mp_address_advertised is a bitmask that records whether
+ * an address has been advertised.
+ *
+ * Initialised in mptcp_subr.c
+ */
+extern int mp_address_count;
+extern struct sockaddr_storage mp_usable_addresses[MAX_ADDRS];
+
+/* Head of reassembly list for a subflow */
+struct sf_reass_queue_head {
+	struct tseg_qent *tqh_first;	/* first element */
+	struct tseg_qent **tqh_last;	/* addr of last next element */
+	struct tseg_qent *disordered;/* first out of order segment in subflow list */
+	STAILQ_ENTRY(sf_reass_queue_head) tqe_next_head;
+};
+
+struct mp_addr_v4 {
+	uint32_t token;
+	in_port_t in_port;
+	uint8_t pos_mask;
+	uint8_t	id;
+	uint8_t is_backup:1;
+	struct	in_addr in_addr;
+};
+
+struct mp_path_map {
+	struct mp_addr_v4 *local_address;
+	struct mp_addr_v4 *remote_address;
+};
+
+TAILQ_HEAD(sf_send_head, sfsendmap_qent);
+STAILQ_HEAD(sf_reass_queues_list_head, sf_reass_queue_head);
+
+/*
+ * Multi-path Session Control Block
+ * XXXNJW: Re-order for better alignment?
+ */
+struct mpcb {
+	struct	inpcb *master_inpcb;	/* back pointer to master internet pcb */
+	struct	tcpcb *master_tcpcb;	/* back pointer to master tcp cb */
+	struct	socket *master_socket;	/* pointer to socket of master subflow */
+
+	struct mtx mpcb_mutex;		/* mutex for the control block */
+
+	struct	callout mpt_rexmt;	/* data-level retransmit callout */
+	int mp_rxtcur;				/* Current data-level RTO in ticks */
+	uint8_t doing_rxmit; 			/* Is a ds-level rexmit required? */
+
+	int mp_added_address_count;	/* added (via add_addr) subflows */
+	int mp_conn_address_count; /* number of addresses available to session */
+	int subflow_cnt;			/* total number of subflows */
+	struct sockaddr_storage mp_added_addresses[MAX_ADDRS];
+
+	/*
+	 * list of reass list heads  for subflows
+	 * create list heads and place into queue, then use pointer from tcpcb.
+	 * Lock access to reass queue list, and individual subflow reass lists
+	 */
+	struct sf_reass_queues_list_head sf_reass_queues_list; /* head of subflow segment list-head list */
+	struct rwlock reass_queues_lock;	/* Lock for list of reassembly list heads, and the subflow segment lists */
+
+	struct sf_send_head sf_sendmap_q; /* subflow send buffer mappings queue */
+	struct dsmapq_head mp_ds_sndmaps;	/* send maps of subflows */
+	struct dsmapq_head mp_rxtmitmaps;	/* maps to be retransmitted */
+
+	SLIST_HEAD(tp_list_head, tcpcb) tp_list; /* list of subflow tcp control blocks */
+
+	struct  mp_addr_v4	loc_addrs[MAX_ADDRS]; /* local addresses available to this connection */
+	uint8_t	laddr_mask;				/* mask of address entries in loc_addrs */
+	struct  mp_addr_v4	rem_addrs[MAX_ADDRS]; /* remote addresses available to this connection */
+	uint8_t	raddr_mask;				/* mask of address entries in rem_addrs*/
+
+	int	mp_state;			/* state of mptcp connection */
+	int	mp_segqlen;			/* segment reassembly queue length */
+	int	mp_dupacks;			/* consecutive dup acks recd */
+
+	volatile int data_wait_count; /* Count of subflows currently stalled waiting for data */
+	uint8_t l_addr_index_cnt;		/* number of entries in local address table */
+	uint8_t r_addr_index_cnt;		/* number of entries in local address table */
+	uint8_t path_cnt;		/* number of entries in local address table */
+
+	uint16_t mp_flags;	// flags
+    uint16_t drexmit_count; /* count of data_level rexmits triggered*/
+
+	uint32_t local_token; 	/* local token for mptcp connection */
+	uint32_t remote_token;  /* token generated by remote host */
+	uint64_t local_key; 	/* 64-bit key sent by this host */
+	uint64_t remote_key; 	/* 64-bit key from remote host */
+
+	uint64_t ds_idsn;		/* data seq initial data seq number */
+	uint64_t ds_snd_una;	/* oldest unacknowledged seq, data space */
+	uint64_t ds_snd_max;	/* max send seq num, data space */
+
+	uint64_t ds_map_max;	/* Seq num of next byte to be mapped to a SF */
+	uint64_t ds_map_min;	/* Lowest seq no. currently in a map (may be below UNA) */
+	uint32_t ds_snd_wnd;	/* current send window (based on rcvd window adv) */
+
+	uint64_t ds_idsr;		/* Initial data sequence receive */
+	uint64_t ds_rcv_nxt;	/* next expected data seq number */
+	uint64_t ds_last_rcvd;	/* the last received ds seq number */
+	uint32_t ds_rcv_wnd;	/* Receive window to be advertised by sub-flows */
+	uint64_t ds_last_dack_sent; /* Last sent data-level ACK */
+
+	uint8_t	mp_status;		/* flags for connection status */
+	uint8_t	direct_map:1,	/* mapping directly to the default subflow */
+			csum_enabled:1,	/* using csum with data segments */
+			passive_open:1,	/* TEMP - are we passive opener */
+			in_def_drop:1;	/* TEMP - are we in deferred drop func */
+
+	uint64_t alpha;			/* alpha value for congestion control */
+	uint64_t total_cwnd;	/* total cwnd of all subflows */
+
+	/*
+	 * The "path manager" of sorts, just an array of addresses
+	 * that have been received via add_addr options.
+	 * The address ID is just the index into the array.
+	 * XXXNJW: This cannot be used for interop, as we cannot
+	 * guarantee a mapping between address IDs and the array indexes
+	 *
+	 * Initialised in mptcp_subr.c
+	 */
+	struct sockaddr_storage mp_path_manager[MAX_ADDRS];
+	uint32_t mp_advaddr_mask; /* Mask for ADD_ADDRs sent */
+	uint32_t mp_advjoin_mask; /* Mask for MP_JOINs sent*/
+
+	struct task	join_task;	/* For enqueuing aysnc joins in swi */
+	struct task	data_task;	/* For enqueuing aysnc subflow data wakeup */
+	struct task pcb_create_task; /* For enqueueing async sf inp creation */
+	struct task	rexmit_task;	/* For enqueuing data-level rexmits */
+};
+
+#define	MPS_CLOSED		0	/* closed */
+#define	MPS_LISTEN		1	/* listening for connection */
+#define	MPS_SYN_SENT		2	/* active, have sent syn */
+#define	MPS_SYN_RECEIVED	3	/* have sent and received syn */
+#define MPS_PRE_ESTABLISHED	4 /* < MPS_ESTABLISHED can't add new subflows */
+#define	MPS_ESTABLISHED		5	/* established */
+#define	MPS_M_FIN_WAIT_1	6
+#define	MPS_M_FIN_WAIT_2	7
+#define MPS_M_CLOSING		8
+#define MPS_M_CLOSE_WAIT	9
+#define MPS_M_LAST_ACK		10
+#define MPS_M_TIME_WAIT		11
+
+
+#define MPO_GOTSYNACK	0x0001
+#define MPO_DSN_MAP	0x0002
+#define	MPO_DATA_ACK	0x0004
+#define MPO_DSN_64	0x0008
+#define MPO_GOTJOINSYNACK	0x0010	/* received a SYN/ACK with MP_JOIN */
+#define MPO_SET_DATA_FIN	0x0020	/* Data-FIN on next DSS */
+#define MPO_SEND_ADD_ADDR	0x0040	/* ADD_ADDR to be send on next packet */
+#define MPO_GOTJOINSYN	0x0080
+
+/* Flags to pass between input/output at subflow-level */
+#define SFF_GOT_SYNACK		0x0001
+#define SFF_GOT_JOIN_SYN	0x0002
+#define SFF_GOT_JOIN_SYNACK	0x0004
+#define SFF_GOT_JOIN_ACK	0x0008
+#define	SFF_DATA_WAIT		0x0010
+#define	SFF_NEED_DACK		0x0020
+#define SFF_FORCE_WIN_UPDATE	0x0040
+#define SFF_SEND_WAIT		0x0080
+
+#define MP_TOKEN_OFFSET	0
+#define MP_IDSN_OFFSET	12
+
+/* offsets for mptcp option parsing */
+#define MP_DSS_FLAGS_OFFSET	3
+#define MP_REMOTE_KEY_OFFSET	4
+#define MP_DATA_ACK_OFFSET 	4
+#define MP_DATA_ACK64_OFFSET	4
+#define MP_DSN_OFFSET			4
+#define MP_SUB_SEQN_OFFSET	8
+#define MP_DATA_LEN_OFFSET	12
+#define MP_CSUM_OFFSET	14
+#define MP_RCV_TOKEN_OFFSET	4
+#define MP_SND_RND_OFFSET	8
+#define MP_SND_MAC_OFFSET	4
+#define MP_SND_RND_SYNACK_OFFSET	12
+#define MP_FAIL_KEY_OFFSET	4
+#define MP_ADD_ADDR_OFFSET	4
+#define MP_ADDID_OFFSET		3
+
+/* MPTCP options */
+#define MPOF_CAPABLE_SYN	0x0001
+#define MPOF_CAPABLE_SYNACK	0x0002
+#define MPOF_CAPABLE_ACK	0x0004
+#define MPOF_MP_CAPABLE	0x0008
+#define MPOF_DSS		0x0010
+#define MPOF_DSN_MAP	0x0020
+#define MPOF_DATA_ACK	0x0040
+#define MPOF_ADD_ADDR	0x0080
+#define MPOF_ADD_ADDR_V4	0x0100
+#define MPOF_MP_JOIN	0x0200
+#define MPOF_MP_RST		0x0400
+#define MPOF_REMOVE_ADDR	0x0800
+#define MPOF_MP_PRIO	0x1000
+#define MPOF_USE_CSUM	0x2000
+#define MPOF_DSN64		0x4000
+#define MPOF_ACK64		0x8000
+#define MPOF_NEED_ACK	0x00010000
+#define MPOF_JOIN_SYN	0x00020000
+#define	MPOF_JOIN_ACK	0x00040000
+#define	MPOF_JOIN_SYNACK	0x00080000
+#define MPOF_BACKUP_PATH	0x00100000
+#define	MPOF_DATA_FIN	0x00200000
+#define MPOF_ADD_ADDR_V6	0x00400000
+#define MPOF_MP_FAIL	0x00800000
+#define MPOF_FASTCLOSE	0x01000000
+#define	MPOF_MAXOPT		0x02000000
+
+/* Bit Mask for D-FIN flag */
+#define MP_SET_DATA_FIN	0x20
+
+/* MPTCP status flags */
+#define MP_INIT 0
+#define CSUM_ENABLED 1
+
+/* mbuf tag defines */
+#define PACKET_TAG_DSN 10
+#define PACKET_COOKIE_MPTCP 34216894
+#define DSN_LEN	8
+
+#define MAPF_IS_SENT	0x00000001 /* Sent all data from map */
+#define MAPF_IS_ACKED	0x00000002 /* All data in map is acknowledged */
+#define MAPF_IS_DUP		0x00000004 /* Duplicate map, already acked at ds-level */
+#define MAPF_IS_REXMIT	0x00000008 /* Is a rexmit of a previously sent map */
+
+#define SB_NUNMAPPED(sb, mp) ((sb)->sb_actual - ((mp)->ds_map_max - (mp)->ds_map_min))
+#define	TCPS_SUBFLOWCLOSING(s)	((s) == TCPS_CLOSE_WAIT)
+
+/*
+ * MP Options. Use struct to hold the first four bytes,
+ * and bit masks to parse out an flags.
+ */
+
+/* MP_CAPABLE option */
+struct mp_capable {
+	uint8_t kind;		/* option kind */
+	uint8_t length;		/* length of option */
+	uint8_t ver_sub;	/* MPTCP version */
+	uint8_t flags;	/* MPTCP subtype */
+
+#define	USE_SHA1	1	/* Bit 1 */
+#define	USE_CSUM	128	/* Bit 8 */
+};
+
+/* DSS option */
+struct mp_dss {
+	uint8_t kind;
+	uint8_t length;
+	uint8_t	subtype;
+	uint8_t dss_flags;
+
+#define ACK_PRESENT	0x0001
+#define ACK_64_PRESENT	0x0002
+#define MAP_PRESENT	0x0004
+#define DSN_64	0x0008
+#define FIN_PRESENT	0x0010
+};
+
+/* MP_JOIN option flags */
+struct mp_join {
+	uint8_t kind;
+	uint8_t length;
+	uint8_t	sub_flags;
+	uint8_t addr_id;
+
+/* masks for bits 16-31 of dss option */
+#define IS_BACKUP	0x01
+};
+
+/* MP_ADD option */
+struct mp_add {
+	uint8_t kind;
+	uint8_t length;
+	uint8_t	sub_ipver;
+	uint8_t addr_id;
+
+#define IS_IPV4	1
+};
+
+struct dsn_tag {
+        struct m_tag tag;
+        uint64_t 	dsn;
+};
+
+struct dss_psdhead {
+	uint64_t dsn;
+	uint32_t ssn;
+	u_short	len;
+	u_short	pad;
+};
+
+typedef struct mptcp_key {
+	uint32_t keylen;
+	uint8_t key[];
+}        mptcp_key_t;
+
+
+/*
+ * LOCK MACROS used for:
+ * sf_reass_queues_list: when user process does reassembly
+ * t_segq: when segments are inserted into the reassembly list
+ */
+#define	SF_REASS_LOCK_INIT(mp)	rw_init(&mp->reass_queues_lock, "reass queues lock")
+#define	SF_REASS_LOCK_DESTROY(mp)	rw_destroy(&mp->reass_queues_lock)
+#define	SF_REASS_WLOCK(mp)	rw_wlock(&mp->reass_queues_lock)
+#define	SF_REASS_WUNLOCK(mp)	rw_wunlock(&mp->reass_queues_lock)
+#define	SF_REASS_RLOCK(mp)	rw_rlock(&mp->reass_queues_lock)
+#define	SF_REASS_RUNLOCK(mp)	rw_runlock(&mp->reass_queues_lock)
+
+#define MPCB_LOCK_INIT(mp) 	mtx_init(&mp->mpcb_mutex, "mpcb", NULL, MTX_DEF | MTX_RECURSE);		// need to add MTX_RECURSE?
+#define MPCB_LOCK(mp)		mtx_lock(&mp->mpcb_mutex)
+#define MPCB_UNLOCK(mp)	mtx_unlock(&mp->mpcb_mutex)
+#define	MPCB_LOCK_ASSERT(mp)	mtx_assert(&mp->mpcb_mutex, MA_OWNED)
+
+
+
+
+/* 64-bit DSN comparisons */
+#define	DSEQ_LT(a,b)	((int64_t)((a)-(b)) < 0)
+#define	DSEQ_LEQ(a,b)	((int64_t)((a)-(b)) <= 0)
+#define	DSEQ_GT(a,b)	((int64_t)((a)-(b)) > 0)
+#define	DSEQ_GEQ(a,b)	((int64_t)((a)-(b)) >= 0)
+
+#define	DSEQ_MIN(a, b)	((SEQ_LT(a, b)) ? (a) : (b))
+#define	DSEQ_MAX(a, b)	((SEQ_GT(a, b)) ? (a) : (b))
+
+/* MP-related debug output */
+
+
+/*
+ * Keep these defines in sync with debug_class struct in mptcp_subr.c
+	{.class = "MPSESSION"},
+	{.class = "DSMAP"}
+*/
+#define MPSESSION 	0x00000001
+#define DSMAP 		0x00000002
+#define SBSTATUS 	0x00000004
+#define REASS		0x00000008
+
+void mp_debug(uint32_t log_class, int msg_verbosity, uint32_t flags, char * fmt, ...);
+
+void	btohex(char *buf, uint32_t buf_len, uint8_t *bytes, int32_t bytes_len, int32_t flags);
+int		mp_output(struct tcpcb *tp);
+void	mp_input(struct tcpcb *tp, struct sockbuf *sb, struct mbuf *m);
+void	ds_reass_init(struct mpcb *m);
+void	mp_order_segment_list(struct tcpcb *tp, struct sockbuf *sb);
+u_int64_t
+		mp_generate_local_key(void);
+uint32_t
+		mp_do_hashing(struct mpcb *mp);
+uint32_t
+		mp_do_sha_hash(uint8_t *digest, uint8_t *key, uint64_t key_length);
+uint32_t
+		mp_get_token(uint8_t *digest);
+uint64_t
+		mp_new_idsn(uint8_t *digest);
+u_short	mp_dss_cksum(struct dss_psdhead);
+uint32_t mp_get_hmac
+		(uint8_t *digest, uint64_t local_key, uint64_t remote_key, uint32_t local_rand, uint32_t remote_rand);
+void	mp_update_map(struct mpcb *mp);
+void	mp_add_local_addr(struct mpcb *mp, struct in_addr *inp_local_addr, in_port_t lport, uint8_t address_id);
+void	mp_add_remote_addr(struct mpcb *mp, struct in_addr *inp_remote_addr, in_port_t rport, uint8_t address_id);
+void	mp_remove_remote_addr(struct mpcb *mp, uint8_t address_id);
+void 	mp_process_local_key(struct mpcb *mp, uint64_t local_key);
+void 	mp_process_remote_key(struct mpcb *mp, uint64_t remote_key);
+
+/* Interface management */
+int     mp_is_if_up(struct sockaddr *l_addr);
+int     mp_is_addr_default(struct sockaddr_in *l_addr, struct mpcb *mp);
+void    mp_update_available_addresses(struct mpcb *mp);
+
+/* tcp_usrreq subflow functions */
+int		tcp_subflow_attach(struct socket *so, struct inpcb *inp, struct mpcb *mp);
+int		tcp_subflow_connect(struct tcpcb *tp, struct sockaddr *nam, struct thread *td);
+
+/* tcp_output related */
+struct ds_map*
+		mp_get_map(struct mpcb *mp, unsigned int sf_mss);
+void
+		mp_insert_map(struct mpcb *mp, struct ds_map *map);
+uint32_t
+		mp_get_recwin(struct mpcb *mp);
+void	mp_update_sndwin(struct mpcb *mp, uint16_t win);
+
+struct ds_map *
+		mp_find_dsmap(struct tcpcb *tp, tcp_seq	seqnum);
+
+/* tcp_input related */
+void	mp_data_ack(struct mpcb *mp, uint64_t data_ack_num);
+void	mp_drop_from_sendbuffer_locked(struct mpcb *mp, int acked, struct ds_map *map);
+void	mp_drop_from_sendbuffer(struct mpcb *mp, int acked, struct ds_map *map);
+void	mp_deferred_sbdrop(struct mpcb *mp);
+void 	mp_init_established(struct tcpcb *tp);
+void	mp_set_ds_map(struct mpcb *mp, uint64_t ds_num);
+
+/* Terminating subflows */
+int		mp_close_subflows(void);
+int		mp_close_subflow(struct tcpcb *);
+void    mp_drop_subflow_tp(struct mpcb *mp, struct tcpcb *tp);
+void	mp_subflow_is_disconnected(struct tcpcb *tp);
+void 	mp_update_subflow_windows(struct tcpcb *tp);
+void	mp_set_cantrcvmore(struct tcpcb *tp);
+
+/* Functions for task queue */
+void	mp_join_task_handler(void *context, int pending);
+void	mp_datascheduler_task_handler(void *context, int pending);
+void 	mp_sf_alloc_task_handler(void *context, int pending);
+void	mp_rexmit_task_handler(void *context, int pending);
+
+/* data_level rexmit */
+void    mp_trigger_rexmit(struct mpcb *mp);
+//void    mp_queue_data_rexmit_unlocked(struct mpcb *mp);
+void    mp_queue_data_rexmit_unlocked(struct tcpcb *tp);
+
+/* To call from debugger */
+void    mp_print_reass_lists(struct mpcb *mp);
+void    mp_print_sqns(struct mpcb *mp);
+void    mp_print_maps(struct mpcb *mp);
+
+#endif /* MPTCP_VAR_H_ */
diff -r 1e55e1aa8b7f sys/netinet/tcp.h
--- a/sys/netinet/tcp.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp.h	Fri Jul 11 13:32:06 2014 +1000
@@ -97,6 +97,7 @@
 #define    TCPOLEN_TSTAMP_APPA		(TCPOLEN_TIMESTAMP+2) /* appendix A */
 #define	TCPOPT_SIGNATURE	19		/* Keyed MD5: RFC 2385 */
 #define	   TCPOLEN_SIGNATURE		18
+#define TCPOPT_MPTCP	30
 
 /* Miscellaneous constants */
 #define	MAX_SACK_BLKS	6	/* Max # SACK blocks stored at receiver side */
diff -r 1e55e1aa8b7f sys/netinet/tcp_input.c
--- a/sys/netinet/tcp_input.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_input.c	Fri Jul 11 13:32:06 2014 +1000
@@ -56,13 +56,16 @@
 #include "opt_ipsec.h"
 #include "opt_tcpdebug.h"
 
+#include <sys/endian.h>
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/hhook.h>
+#include <sys/limits.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
 #include <sys/proc.h>		/* for proc0 declaration */
 #include <sys/protosw.h>
+#include <sys/sbuf.h>
 #include <sys/sdt.h>
 #include <sys/signalvar.h>
 #include <sys/socket.h>
@@ -70,6 +73,12 @@
 #include <sys/sysctl.h>
 #include <sys/syslog.h>
 #include <sys/systm.h>
+#include <sys/kdb.h>
+#include <sys/taskqueue.h>
+
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/limits.h>
 
 #include <machine/cpu.h>	/* before tcp_seq.h, for tcp_random18() */
 
@@ -112,6 +121,12 @@
 #include <netinet/tcp_offload.h>
 #endif
 
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/sctp_pcb.h>
+#include <netinet/sctp_auth.h>
+#include <netinet/sctp_constants.h>
+
 #ifdef IPSEC
 #include <netipsec/ipsec.h>
 #include <netipsec/ipsec6.h>
@@ -134,7 +149,7 @@
     &VNET_NAME(blackhole), 0,
     "Do not send RST on segments to closed ports");
 
-VNET_DEFINE(int, tcp_delack_enabled) = 1;
+VNET_DEFINE(int, tcp_delack_enabled) = 0;
 SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, delayed_ack, CTLFLAG_RW,
     &VNET_NAME(tcp_delack_enabled), 0,
     "Delay ACK to try and piggyback it onto a data packet");
@@ -215,6 +230,17 @@
     &VNET_NAME(tcp_autorcvbuf_max), 0,
     "Max size of automatic receive buffer");
 
+VNET_DEFINE(int, tcp_do_mptcp) = 1;
+#define	V_tcp_do_mptcp	VNET(tcp_do_mptcp)
+SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, mptcp, CTLFLAG_RW,
+    &VNET_NAME(tcp_do_mptcp), 0,
+    "Enable Multipath Support");
+
+MALLOC_DEFINE(M_DSSMAP, "dssmap", "Struct to hold dss maps");
+
+static VNET_DEFINE(uma_zone_t, tcp_unmapped_zone);
+#define	V_tcp_unmapped_zone		VNET(tcp_unmapped_zone)
+
 VNET_DEFINE(struct inpcbhead, tcb);
 #define	tcb6	tcb  /* for KAME src sync over BSD*'s */
 VNET_DEFINE(struct inpcbinfo, tcbinfo);
@@ -228,7 +254,7 @@
 static void	 tcp_pulloutofband(struct socket *,
 		     struct tcphdr *, struct mbuf *, int);
 static void	 tcp_xmit_timer(struct tcpcb *, int);
-static void	 tcp_newreno_partial_ack(struct tcpcb *, struct tcphdr *);
+static void	 tcp_newreno_partial_ack(struct tcpcb *, struct tcphdr *, uint32_t);
 static void inline 	tcp_fields_to_host(struct tcphdr *);
 #ifdef TCP_SIGNATURE
 static void inline 	tcp_fields_to_net(struct tcphdr *);
@@ -236,20 +262,27 @@
 			    int, struct tcpopt *, struct tcphdr *, u_int);
 #endif
 static void inline	cc_ack_received(struct tcpcb *tp, struct tcphdr *th,
-			    uint16_t type);
+			    uint16_t type, uint32_t acked);
 static void inline	cc_conn_init(struct tcpcb *tp);
 static void inline	cc_post_recovery(struct tcpcb *tp, struct tcphdr *th);
 static void inline	hhook_run_tcp_est_in(struct tcpcb *tp,
 			    struct tcphdr *th, struct tcpopt *to);
 
 /*
- * TCP statistics are stored in an "array" of counter(9)s.
+ * MPTCP functions for handling new MPTCP-generated TCP options and
+ * mptcp connection states
  */
+static void inline mp_dosubtypes(struct tcpopt *to, uint8_t subtype,
+		u_char *cp, int opt, int optlen, int flags);
+static uint64_t inline mp_get_ds_64_upper(struct mpcb *mp);
+/* TCP statistics are stored in an "array" of counter(9)s. */
 VNET_PCPUSTAT_DEFINE(struct tcpstat, tcpstat);
 VNET_PCPUSTAT_SYSINIT(tcpstat);
 SYSCTL_VNET_PCPUSTAT(_net_inet_tcp, TCPCTL_STATS, stats, struct tcpstat,
     tcpstat, "TCP statistics (struct tcpstat, netinet/tcp_var.h)");
 
+#define MSBIT_CHECK 0x80000000
+
 #ifdef VIMAGE
 VNET_PCPUSTAT_SYSUNINIT(tcpstat);
 #endif /* VIMAGE */
@@ -264,6 +297,316 @@
 	counter_u64_add(VNET(tcpstat)[statnum], 1);
 }
 
+
+/*
+ * Zone for storing tcp segments that arrive and do not belong to an existing
+ * DS-mapping. We need to buffer these segments until the mapping arrives
+ * (the segment with the mapping may have been lost).
+ *
+ * XXXNJW: Need an appropriate tunable for setting the max size of this zone
+ */
+void
+tcp_unmapped_init(void)
+{
+	V_tcp_unmapped_zone = uma_zcreate("tcpunmapped", sizeof (struct tseg_qent),
+	    NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
+	/* Set the zone limit and read back the effective value. */
+	uma_zone_set_max(V_tcp_unmapped_zone, 100);
+}
+
+void
+tcp_unmapped_flush(struct tcpcb *tp)
+{
+	struct tseg_qent *qe;
+
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+	while ((qe = TAILQ_FIRST(tp->t_unmapped_segq)) != NULL) {
+		TAILQ_REMOVE(tp->t_segq, qe, tqe_q);
+		m_freem(qe->tqe_m);
+		uma_zfree(V_tcp_unmapped_zone, qe);
+	}
+}
+
+static inline uint64_t
+mp_get_ds_64_upper(struct mpcb *mp) {
+	return (mp->ds_map_max & 0xFFFFFFFF00000000);
+}
+
+static void inline
+mp_dosubtypes(struct tcpopt *to, uint8_t subtype, u_char *cp, int opt, int optlen, int flags) {
+	int byte_offset;
+	struct mp_capable mcap;
+	struct mp_join mjoin;
+
+	subtype = subtype >> 4;
+
+	switch(subtype)
+	{
+	case MPTCP_SUBTYPE_MP_CAPABLE:
+	{
+		/* XXXNJW:
+		 * have to check that the 's' bit is set
+		 * also should check for the dss csum bit (for now just ignore it...)
+		 */
+		if (optlen != MPTCP_SUBLEN_MP_CAPABLE_SYN &&
+				optlen != MPTCP_SUBLEN_MP_CAPABLE_ACK) {
+			break;
+		}
+
+		/* indicate we have mptcp options present */
+		to->to_flags |= TOF_MPTCP;
+		to->to_mopts.mpo_flags |= MPOF_MP_CAPABLE;
+
+		if ((flags & TO_SYN) &&
+				(optlen == MPTCP_SUBLEN_MP_CAPABLE_SYN)) {
+
+				bcopy((char *)cp,(char *)&mcap,
+						sizeof(struct mp_capable));
+
+				/* using checksums? */
+				if (mcap.flags & USE_CSUM) {
+					to->to_mopts.mpo_flags |= MPOF_USE_CSUM;
+				}
+
+				/* need to save the incoming key */
+				bcopy((char *)cp + MP_REMOTE_KEY_OFFSET,
+						(char *)&to->to_mopts.remote_key,
+						sizeof(to->to_mopts.remote_key));
+		}
+
+		if (optlen == MPTCP_SUBLEN_MP_CAPABLE_ACK) {
+			/* do some validation here */
+		}
+		break;
+	}
+	case MPTCP_SUBTYPE_DSS:
+	{
+			byte_offset = 0;
+			uint8_t	dss_flags;
+
+			to->to_mopts.mpo_flags |= MPOF_DSS;
+			bcopy((char *)cp + MP_DSS_FLAGS_OFFSET,(char *)&dss_flags,
+							sizeof(uint8_t));
+
+			/* Data-FIN present */
+			if(dss_flags & FIN_PRESENT)
+				to->to_mopts.mpo_flags |= MPOF_DATA_FIN;
+
+			/* Data-ACK present */
+			if (dss_flags & ACK_PRESENT) {
+				// set flag that we have an ACK
+				to->to_mopts.mpo_flags |= MPOF_DATA_ACK;
+
+				/* Is 64-bit Data ACK present */
+				if (dss_flags & ACK_64_PRESENT) {
+					to->to_mopts.mpo_flags |= MPOF_ACK64;
+					// grabbing whole 64-bits
+					bcopy((char *)cp + MP_DATA_ACK64_OFFSET,
+						(char *)&to->to_mopts.data_ack_num,
+						sizeof(to->to_mopts.data_ack_num));
+					to->to_mopts.data_ack_num =
+							be64toh(to->to_mopts.data_ack_num);
+					// offset the presence of 64-bit ACK
+					byte_offset += 8;
+				} else {
+					uint32_t short_data_ack;
+					bcopy((char *)cp + MP_DATA_ACK_OFFSET,
+						(char *)&short_data_ack,
+						sizeof(short_data_ack));
+					to->to_mopts.data_ack_num = ntohl(short_data_ack);
+					byte_offset += 4; // offset the presence of 32-bit ACK
+				}
+			}
+
+			/* DSN, SSN, length and csum present */
+			if (dss_flags & MAP_PRESENT) {
+				/* flag the new DSN Mapping for this subflow */
+				to->to_mopts.mpo_flags |= MPOF_DSN_MAP;
+
+				/* Is DSN 8 octets */
+				if (dss_flags & DSN_64) {
+					bcopy((char *)cp + MP_DSN_OFFSET + byte_offset,
+						(char *)&to->to_mopts.data_seq_num,
+						sizeof(to->to_mopts.data_seq_num));
+					to->to_mopts.data_seq_num =
+					    be64toh(to->to_mopts.data_seq_num);
+					byte_offset += 4;
+					to->to_mopts.mpo_flags |= MPOF_DSN64;
+				} else {
+					uint32_t short_dsn;
+					bcopy((char *)cp + MP_DSN_OFFSET + byte_offset,
+						(char *)&short_dsn,
+						sizeof(short_dsn));
+					to->to_mopts.data_seq_num = (uint64_t) ntohl(short_dsn);
+				}
+
+				/* subflow sequence number */
+				bcopy((char *)cp + MP_SUB_SEQN_OFFSET + byte_offset,
+					(char *)&to->to_mopts.sub_seq_num,
+					sizeof(to->to_mopts.sub_seq_num));
+				to->to_mopts.sub_seq_num = ntohl(to->to_mopts.sub_seq_num);
+
+				/* Data-level length */
+				bcopy((char *)cp + MP_DATA_LEN_OFFSET + byte_offset,
+					(char *)&to->to_mopts.dss_data_len,
+					sizeof(to->to_mopts.dss_data_len));
+				to->to_mopts.dss_data_len = ntohs(to->to_mopts.dss_data_len);
+
+				/*
+				 * XXXNJW: need to check if using csums
+				 * should do this by checking the length of the option
+				 * at the start and keeping track along the way....
+				 */
+
+				 /* checksum */
+//					bcopy((char *)cp + MP_CSUM_OFFSET + byte_offset,
+//						(char *)&to->to_mopts.dss_csum,
+//						sizeof(to->to_mopts.dss_csum));
+//					to->to_mopts.dss_csum = ntohs(to->to_mopts.dss_csum);
+			}
+			break;
+	}
+	case MPTCP_SUBTYPE_MP_JOIN:
+	{
+		if (optlen != MPTCP_SUBLEN_MP_JOIN_SYN &&
+			optlen != MPTCP_SUBLEN_MP_JOIN_SYNACK &&
+			optlen != MPTCP_SUBLEN_MP_JOIN_ACK) {
+			break;
+		}
+
+		/* can use optlen to differentiate subtype variations */
+		to->to_mopts.optlen = optlen;
+
+		switch (optlen)
+		{
+		case MPTCP_SUBLEN_MP_JOIN_SYN:
+			to->to_flags |= TOF_MPTCP;
+
+			/* copy address id */
+			bcopy((char *)cp + 3,
+					(char *)&to->to_mopts.addr_id,
+					sizeof(to->to_mopts.addr_id));
+
+			/* is this a backup path */
+			if (subtype & IS_BACKUP) {
+				to->to_mopts.mpo_flags |= MPOF_BACKUP_PATH;
+			}
+
+			/* save the local_token */
+			bcopy((char *)cp + MP_RCV_TOKEN_OFFSET,
+					(char *)&to->to_mopts.rcv_token,
+					sizeof(to->to_mopts.rcv_token));
+
+			/* save the senders random number */
+			bcopy((char *)cp + MP_SND_RND_OFFSET,
+					(char *)&to->to_mopts.snd_rnd,
+					sizeof(to->to_mopts.snd_rnd));
+			to->to_mopts.mpo_flags |= MPOF_JOIN_SYN;
+			break;
+		case MPTCP_SUBLEN_MP_JOIN_SYNACK:
+			to->to_flags |= TOF_MPTCP;
+
+			/* copy option 'header' into struct */
+			bcopy((char *)cp,(char *)&mjoin,
+				sizeof(struct mp_join));
+			/* is this a backup path */
+			if (mjoin.sub_flags & IS_BACKUP) {
+				to->to_mopts.mpo_flags |= MPOF_BACKUP_PATH;
+			}
+			/* save remote truncate MAC */
+			bcopy((char *)cp + MP_SND_MAC_OFFSET,
+			    (char *)&to->to_mopts.snd_trc_mac,
+			    sizeof(to->to_mopts.snd_trc_mac));
+
+			/* save the senders random number */
+			bcopy((char *)cp + MP_SND_RND_SYNACK_OFFSET,
+			    (char *)&to->to_mopts.snd_rnd,
+			    sizeof(to->to_mopts.snd_rnd));
+
+			to->to_mopts.mpo_flags |= MPOF_JOIN_SYNACK;
+			break;
+		case MPTCP_SUBLEN_MP_JOIN_ACK:
+			to->to_flags |= TOF_MPTCP;
+
+			/* just take a pointer to the hmac. We can copy the bytes in
+			 * syncache_expand_subflow as the memory will still be valid */
+			to->to_mopts.snd_mac = (char *)cp + MP_SND_MAC_OFFSET;
+
+			to->to_mopts.mpo_flags |= MPOF_JOIN_ACK;
+			to->to_mopts.mpo_flags |= MPOF_NEED_ACK;
+			break;
+		}
+		break;
+	}
+	case MPTCP_SUBTYPE_ADD_ADDR:
+	{
+		/* not using the port number field for now */
+		if (optlen != MPTCP_SUBLEN_ADD_ADDRV4 &&
+		    optlen != MPTCP_SUBLEN_ADD_ADDRV6)
+				break;
+
+		void * addr = NULL;
+		int addr_len = 0;
+
+		switch (optlen)
+		{
+		case MPTCP_SUBLEN_ADD_ADDRV4:
+			to->to_mopts.mpo_flags |= MPOF_ADD_ADDR_V4;
+			addr = &((struct sockaddr_in *)
+					&to->to_mopts.new_addr)->sin_addr.s_addr;
+			addr_len = 4;
+			/* Copy address ID */
+			break;
+		case MPTCP_SUBLEN_ADD_ADDRV6:
+			to->to_mopts.mpo_flags |= MPOF_ADD_ADDR_V6;
+			/* Copy INET address */
+			addr = &((struct sockaddr_in6 *)
+					&to->to_mopts.new_addr)->sin6_addr.__u6_addr;
+			addr_len = 16;
+			break;
+		}
+		/* Copy INET address */
+		bcopy((char *)cp + MP_ADD_ADDR_OFFSET, addr,
+		    addr_len);
+
+		char buf[128];
+		inet_ntop(AF_INET, &(((struct in_addr *) addr)->s_addr), buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "option add_addr %s\n", buf);
+
+		/* Copy address ID */
+		bcopy((char *)cp + MP_ADDID_OFFSET,
+		    (char *)&to->to_mopts.addr_id,
+		    sizeof(to->to_mopts.addr_id));
+		break;
+	}
+	case MPTCP_SUBTYPE_REMOVE_ADDR:
+	{
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_PRIO:
+	{
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_FAIL:
+	{
+
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_FASTCLOSE:
+	{
+		if (optlen != MPTCP_SUBTYPELEN_MP_FASTCLOSE)
+			break;
+
+		to->to_mopts.mpo_flags |= MPOF_FASTCLOSE;
+
+		bcopy((char *)cp + MP_FAIL_KEY_OFFSET,
+			(char *)&to->to_mopts.local_key,
+			sizeof(&to->to_mopts.local_key));
+		break;
+	}
+	} /* end of switch */
+}
+
 /*
  * Wrapper for the TCP established input helper hook.
  */
@@ -286,11 +629,12 @@
  * CC wrapper hook functions
  */
 static void inline
-cc_ack_received(struct tcpcb *tp, struct tcphdr *th, uint16_t type)
+cc_ack_received(struct tcpcb *tp, struct tcphdr *th, uint16_t type,
+    uint32_t acked)
 {
 	INP_WLOCK_ASSERT(tp->t_inpcb);
 
-	tp->ccv->bytes_this_ack = BYTES_THIS_ACK(tp, th);
+	tp->ccv->bytes_this_ack = acked;
 	if (tp->snd_cwnd <= tp->snd_wnd)
 		tp->ccv->flags |= CCF_CWND_LIMITED;
 	else
@@ -868,8 +1212,20 @@
 		    th->th_sport, ip->ip_dst, th->th_dport,
 		    INPLOOKUP_WILDCARD | INPLOOKUP_WLOCKPCB,
 		    m->m_pkthdr.rcvif, m);
+
 #endif /* INET */
 
+
+	if (thflags & TH_SYN) {
+		/* debug output for addresses */
+		char buf[128];
+		inet_ntop(AF_INET, &ip->ip_src, buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "%s: SYN from %s port %d to ", __func__,
+			buf, ntohs(th->th_sport));
+		inet_ntop(AF_INET, &ip->ip_dst, buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "%s port %d\n", buf, ntohs(th->th_dport));
+	}
+
 	/*
 	 * If the INPCB does not exist then all data in the incoming
 	 * segment is discarded and an appropriate RST is sent back.
@@ -919,6 +1275,7 @@
 	}
 #endif /* IPSEC */
 
+
 	/*
 	 * Check the minimum TTL for socket.
 	 */
@@ -1044,6 +1401,7 @@
 		tcp_savetcp = *th;
 	}
 #endif /* TCPDEBUG */
+
 	/*
 	 * When the socket is accepting connections (the INPCB is in LISTEN
 	 * state) we look into the SYN cache if this is a new connection
@@ -1051,8 +1409,12 @@
 	 * sockets are never in TCPS_ESTABLISHED, the V_tcbinfo lock will be
 	 * held in this case.
 	 */
-	if (so->so_options & SO_ACCEPTCONN) {
+	if ((so->so_options & SO_ACCEPTCONN)
+	    || (tp->t_mpcb->mp_state == MPS_ESTABLISHED && !IS_MASTER_TP(tp)
+	    && tp->t_state == TCPS_LISTEN
+	    && (so->so_options & SO_ACCEPTCONN) == 0)) {	//XXXNJW this is not pretty
 		struct in_conninfo inc;
+		int is_subflow = 0;
 
 		KASSERT(tp->t_state == TCPS_LISTEN, ("%s: so accepting but "
 		    "tp not listening", __func__));
@@ -1080,17 +1442,38 @@
 		 * socket appended to the listen queue in SYN_RECEIVED state.
 		 */
 		if ((thflags & (TH_RST|TH_ACK|TH_SYN)) == TH_ACK) {
+			int expand_result = 0;
 			/*
 			 * Parse the TCP options here because
 			 * syncookies need access to the reflected
 			 * timestamp.
 			 */
 			tcp_dooptions(&to, optp, optlen, 0);
+
+			/*
+			 * check for an mp_join option on the ACK. this means
+			 * that the handshake is for a mptcp subflow - thus
+			 * the path from here on should be different.
+			 *
+			 * might need to alter syncache_expand (dont't actually
+			 * want to allocate a new socket for a subflow, as it
+			 * should be associated with the socket of the master_tp
+			 *
+			 */
+
 			/*
 			 * NB: syncache_expand() doesn't unlock
 			 * inp and tcpinfo locks.
 			 */
-			if (!syncache_expand(&inc, &to, th, &so, m)) {
+			if (to.to_mopts.mpo_flags & MPOF_JOIN_ACK) {
+				expand_result =
+				    syncache_expand_subflow(&inc, &to, th, &so, m);
+				is_subflow = 1;
+			} else {
+				expand_result = syncache_expand(&inc, &to, th, &so, m);
+			}
+
+			if(!expand_result) {
 				/*
 				 * No syncache entry or ACK was not
 				 * for our SYN/ACK.  Send a RST.
@@ -1100,6 +1483,7 @@
 				rstreason = BANDLIM_RST_OPENPORT;
 				goto dropwithreset;
 			}
+
 			if (so == NULL) {
 				/*
 				 * We completed the 3-way handshake
@@ -1124,17 +1508,27 @@
 				} else
 					goto dropunlock;
 			}
-			/*
-			 * Socket is created in state SYN_RECEIVED.
-			 * Unlock the listen socket, lock the newly
-			 * created socket and update the tp variable.
-			 */
-			INP_WUNLOCK(inp);	/* listen socket */
-			inp = sotoinpcb(so);
-			INP_WLOCK(inp);		/* new connection */
-			tp = intotcpcb(inp);
+
+			INP_WUNLOCK(inp); /* listen socket */
+
+			/* need to get the inp for the sublfow */
+			if (is_subflow) {
+				tp = SLIST_FIRST(&intotcpcb(sotoinpcb(so))->t_mpcb->tp_list);
+			} else {
+				tp = intotcpcb(sotoinpcb(so));
+			}
+
+//			printf("%s: got SYN, printing tp pointers\n", __func__);
+//			struct tcpcb *sf_tp = NULL;
+//			SLIST_FOREACH(sf_tp, &intotcpcb(sotoinpcb(so))->t_mpcb->tp_list,
+//			    next_subflow) {
+//				printf("%s: sf_tp %p\n", __func__, sf_tp);
+//			}
+
+			INP_WLOCK(tp->t_inpcb); /* new connection */
 			KASSERT(tp->t_state == TCPS_SYN_RECEIVED,
-			    ("%s: ", __func__));
+			    ("%s: expanded tp not in SYN_RECEIVED", __func__));
+
 #ifdef TCP_SIGNATURE
 			if (sig_checked == 0)  {
 				tcp_dooptions(&to, optp, optlen,
@@ -1165,6 +1559,8 @@
 			INP_INFO_UNLOCK_ASSERT(&V_tcbinfo);
 			return;
 		}
+		/* end syncache check on ACK */
+
 		/*
 		 * Segment flag validation for new connection attempts:
 		 *
@@ -1356,7 +1752,9 @@
 			tcp_trace(TA_INPUT, ostate, tp,
 			    (void *)tcp_saveipgen, &tcp_savetcp, 0);
 #endif
+
 		tcp_dooptions(&to, optp, optlen, TO_SYN);
+
 		syncache_add(&inc, &to, th, inp, &so, m, NULL, NULL);
 		/*
 		 * Entry added to syncache and mbuf consumed.
@@ -1460,14 +1858,16 @@
     struct tcpcb *tp, int drop_hdrlen, int tlen, uint8_t iptos,
     int ti_locked)
 {
-	int thflags, acked, ourfinisacked, needoutput = 0;
-	int rstreason, todrop, win;
+	int thflags, acked = 0, ourfinisacked, oursynisacked = 0, needoutput = 0;
+	int rstreason = 0, todrop, win;
 	u_long tiwin;
 	char *s;
 	struct in_conninfo *inc;
-	struct mbuf *mfree;
 	struct tcpopt to;
 
+	struct mpcb *mp = tp->t_mpcb;
+	struct ds_map* map = NULL;
+
 #ifdef TCPDEBUG
 	/*
 	 * The size of tcp_saveipgen must be the size of the max ip header,
@@ -1523,7 +1923,24 @@
 	 * Unscale the window into a 32-bit value.
 	 * For the SYN_SENT state the scale is zero.
 	 */
-	tiwin = th->th_win << tp->snd_scale;
+	if (!tp->mp_active)
+		tiwin = th->th_win << tp->snd_scale;
+	else
+		tiwin = tp->t_mpcb->ds_snd_wnd << tp->snd_scale;;
+
+	// XXXNJW connection wide rcv window
+	// tiwin should be set to the mp level window.
+
+	/*
+	 * RST will not have the ACK field set (th_ack == 0). Check for TH_RST
+	 * as calling BYTES_THIS_ACK with zero will trigger a false wrap detection.
+	 */
+	if (((thflags & TH_RST) == 0) && (th->th_flags & TH_ACK))
+		acked = BYTES_THIS_ACK(tp, th);
+
+	if (thflags & TH_RST) {
+		acked = 0;
+	}
 
 	/*
 	 * TCP ECN processing.
@@ -1587,11 +2004,485 @@
 	}
 
 	/*
+	 * Do we need to send a data-level ACK in response to this segment?
+	 */
+	if (to.to_mopts.mpo_flags & MPOF_NEED_ACK) {
+		tp->sf_flags |= SFF_NEED_DACK;
+		tp->t_flags |= TF_ACKNOW;
+	}
+
+	/*
+	 * Got MP_JOIN option. If the length is correct tcp_output
+	 * will check this flag for MPOF_MP_JOIN and send a SYNACK
+	 * with MP_JOIN option. Otherwise just discard the segment.
+	 */
+	if (to.to_mopts.mpo_flags & MPOF_JOIN_SYN) {
+		mp_debug(MPSESSION, 2, 0, "%s: got JOIN_SYN\n", __func__);
+		/*
+		 * Got a MP_JOIN_SYN option. Need to send a JOIN_SYNACK
+		 * in reponse.
+		 */
+		if (to.to_mopts.optlen != MPTCP_SUBLEN_MP_JOIN_SYN) {
+			to.to_mopts.mpo_flags &= ~MPOF_MP_JOIN;
+			goto dropwithreset;
+		}
+
+		/* Syncache respond needs these for HMAC
+		 * tp for subflow is not in use at this point */
+		to.to_mopts.remote_key = mp->remote_key;
+		to.to_mopts.local_key = mp->local_key;
+
+		tp->sf_flags |= SFF_GOT_JOIN_SYN;
+	}
+	else if (to.to_mopts.mpo_flags & MPOF_JOIN_ACK) {
+		if (to.to_mopts.optlen == MPTCP_SUBLEN_MP_JOIN_ACK) {
+			/*
+			 * Got an ACK with MP_JOINACK in it. Go straight to established
+			 * since multipath is already active. We will also be sending an
+			 * ACK for this packet, which will put the active opener into
+			 * established for this subflow.
+			 */
+			tp->t_flags |= TF_ACKNOW;
+			tp->sf_flags |= SFF_GOT_JOIN_ACK;
+
+			//mp_init_established(tp);	//XXXNJW really worth a function call?
+			/* We can set the additional subflow as active right from here */
+			tp->mp_active = 1;
+			tp->infinite_map = 0;
+//			atomic_add_int(&tp->t_mpcb->subflow_cnt,1);
+			mp_debug(MPSESSION, 1, 0,"%s:%d new mp connection for tp: %p: \n",
+			    __func__, __LINE__, tp);
+			mp_debug(MPSESSION, 1, 0, "%s: subflow tp %p (slave to master tp %p) "
+			    "transitioning to MPS_ESTABLISHED\n", __func__, tp,
+			    tp->t_mpcb->master_tcpcb);
+		}
+	} /* End join processing */
+
+	/*
+	 * Got a new DSS mapping for this subflow.
+	 */
+	if ((to.to_mopts.mpo_flags & MPOF_DSS)) {
+		/*
+		 * Convert 32bit DS map start sequence numbers into 64 bit.
+		 */
+		if ((to.to_mopts.mpo_flags & MPOF_DSN64) == 0 &&
+		    (to.to_mopts.mpo_flags & MPOF_DSN_MAP)) {
+
+			/* XXX: need to think about what value to key the if block off
+			 * that doesn't require checking into mpcb */
+			if(tp->infinite_map) {
+				to.to_mopts.data_seq_num =
+				    (tp->t_mpcb->ds_idsr & 0xFFFFFFFF00000000) |
+					(uint32_t) to.to_mopts.data_seq_num;
+			} else {
+				to.to_mopts.data_seq_num =
+					(((tp->t_mpcb->ds_rcv_nxt) & 0xFFFFFFFF00000000)
+					| (uint32_t) to.to_mopts.data_seq_num);
+				if ((uint32_t)tp->t_mpcb->ds_rcv_nxt >
+				    (uint32_t)to.to_mopts.data_seq_num
+					&& ((uint32_t)to.to_mopts.data_seq_num & 0xC0000000) == 0
+					&& ((uint32_t)tp->t_mpcb->ds_rcv_nxt & 0xC0000000)
+					== 0xC0000000) {
+					mp_debug(MPSESSION, 4, 0, "%s: detected wrap on lower 32 "
+							"of ds_rcv_nxt %ju: %u, dsn map start %ju : %u\n",
+					    __func__, tp->t_mpcb->ds_rcv_nxt,
+					    (uint32_t)tp->t_mpcb->ds_rcv_nxt,
+					    to.to_mopts.data_seq_num,
+					    (uint32_t)to.to_mopts.data_seq_num);
+					to.to_mopts.data_seq_num += ((uint64_t)1 << 32);
+				}
+			}
+		}
+
+		if ((to.to_mopts.mpo_flags & MPOF_ACK64) == 0 &&
+		    (to.to_mopts.mpo_flags & MPOF_DATA_ACK)) {
+
+			/* We check the window field only on  D-ACK packets
+			 * if the window has changed, set tiwin to new window
+			 * and set the new window.
+			 */
+			if (th->th_win != tp->t_mpcb->ds_snd_wnd) {
+				tiwin = th->th_win << tp->snd_scale;
+				mp_update_sndwin(tp->t_mpcb, th->th_win);
+			}
+
+			if(tp->infinite_map)
+				to.to_mopts.data_ack_num = (tp->t_mpcb->ds_idsn & 0xFFFFFFFF00000000)
+					| (uint32_t) to.to_mopts.data_ack_num;
+			else {
+				to.to_mopts.data_ack_num =
+					(((tp->t_mpcb->ds_snd_una) & 0xFFFFFFFF00000000)
+					| (uint32_t) to.to_mopts.data_ack_num);
+				if ((uint32_t)tp->t_mpcb->ds_snd_una > (uint32_t)to.to_mopts.data_ack_num
+						&& ((uint32_t)to.to_mopts.data_ack_num & 0xC0000000) == 0
+						&& ((uint32_t)tp->t_mpcb->ds_snd_una & 0xC0000000) == 0xC0000000) {
+					mp_debug(MPSESSION, 4, 0, "%s: detected wrap on lower 32 of"
+							" ds_snd_una %ju : %u, data ack %ju : %u\n",
+					    __func__, tp->t_mpcb->ds_snd_una,
+					    (uint32_t)tp->t_mpcb->ds_snd_una,
+					    to.to_mopts.data_ack_num,
+					    (uint32_t)to.to_mopts.data_ack_num);
+					to.to_mopts.data_ack_num += ((uint64_t)1 << 32);
+				}
+			}
+
+		}
+
+		/*
+		 * XXXNJW: infinite map = 1 for new flows, so when we get the
+		 * first DSN on an active opener we can use it to set our
+		 * initial rcv_nxt. BUT what if we want to specify our own
+		 * ds_rcv number space (by advertising a D-ACK....)
+		 */
+
+		if (to.to_mopts.mpo_flags & MPOF_DATA_FIN) {
+			//tp->t_state = TCPS_CLOSE_WAIT;			//XXXNJW TODO
+		}
+
+		if (to.to_mopts.mpo_flags & MPOF_DSN_MAP) {
+			/* got a DSN with no length, drop to infinite mapping */
+			if (to.to_mopts.dss_data_len == 0)
+				panic("dropping back to infinite map not implemented\n");
+
+			struct ds_map *temp_map, *temp_map2, *next_map = NULL;
+			int found_dup_map = 0;
+
+			/* find the map that this segment belongs to */
+			TAILQ_FOREACH_SAFE(temp_map, &tp->t_rxdsmaps, sf_ds_map_next,
+					temp_map2) {
+
+				/* This is a completed map, so remove it from t_rxdmaps */
+				if (SEQ_GEQ(tp->rcv_nxt,temp_map->sf_seq_start
+					+ temp_map->ds_map_len)) {
+					mp_debug(DSMAP, 4, 0, "remove map: sf_seq_start: %u. "
+							"end: %u\n", temp_map->sf_seq_start,
+							temp_map->sf_seq_start + temp_map->ds_map_len);
+					TAILQ_REMOVE(&tp->t_rxdsmaps, temp_map, sf_ds_map_next);
+					free(temp_map, M_DSSMAP);
+					continue;
+				}
+
+				if (!next_map) {
+					tcp_seq new_sf_map_start =
+					    to.to_mopts.sub_seq_num + tp->irs;
+					tcp_seq temp_map_end =
+					    temp_map->sf_seq_start + temp_map->ds_map_len;
+
+					/* A segment part-way into an already recv'd mapping? */
+//					if (new_sf_map_start > temp_map->sf_seq_start &&
+//						to.to_mopts.data_seq_num == temp_map->ds_map_start) {
+					// XXXNJW this would be a broken mapping
+					// (SSN, DSN) out of sync
+
+					/* is this map fully contained within an existing map? */
+					if (new_sf_map_start > temp_map->sf_seq_start &&
+							(new_sf_map_start + to.to_mopts.dss_data_len <=
+							temp_map->sf_seq_start + temp_map->ds_map_len)) {
+
+						int map_offset = 0;
+						map_offset = new_sf_map_start - temp_map->sf_seq_start;
+
+						/* double check that DSNs are aligned, if so set dup */
+						if (temp_map->ds_map_start + map_offset ==
+						    to.to_mopts.data_seq_num) {
+							found_dup_map = 1;
+							map = temp_map;
+						}
+
+					}
+
+					/* An unseen map, therefore should be inserted before
+					 * temp_map in our map list */
+					if (new_sf_map_start >= temp_map_end &&
+						(temp_map_end > temp_map->sf_seq_start ||
+						(new_sf_map_start & 0xC0000000) == 0)) {
+						next_map = temp_map;
+					}
+
+					/* A duplicate of a map we have in the list */
+					if (to.to_mopts.data_seq_num == temp_map->ds_map_start) {
+						mp_debug(DSMAP, 4, 0, "%s: : sqn %u, dup map: %u "
+							"t_len: %d\n", __func__, th->th_seq,
+							(uint32_t) to.to_mopts.data_seq_num, tlen);
+						KASSERT(new_sf_map_start == temp_map->sf_seq_start
+							//&& to.to_mopts.dss_csum == temp_map->ds_map_csum
+							,("DSN number matches existing map "
+							 "but SSNs are not the same\n"));
+
+//						KASSERT(to.to_mopts.dss_data_len == temp_map->ds_map_len
+//								//&& to.to_mopts.dss_csum == temp_map->ds_map_csum
+//								,("SF sequence number matches existing map "
+//								 "but maps are not the same\n"));
+						found_dup_map = 1;
+						map = temp_map;
+					}
+				}
+
+			}
+
+
+			/*
+			 * XXXNJW: Need to think about coalescing mappings that overlap on
+			 * the right hand edge.
+			 */
+
+			/*
+			 * If we do not find a map covered by this sequence number,
+			 * allocate a new map and put into mapping list.
+			 * Also for cases where !has_dsn_map (map will be NULL
+			 * in this case)
+			 */
+			if (!found_dup_map) {
+
+				/* XXX: should use zone allocator */
+				map = malloc(sizeof(struct ds_map), M_DSSMAP, M_NOWAIT);
+
+				/* set values for mapping */
+				map->ds_map_start = to.to_mopts.data_seq_num;
+				map->sf_seq_start = to.to_mopts.sub_seq_num + tp->irs;
+				map->ds_map_len = map->ds_map_offset =
+					to.to_mopts.dss_data_len;
+				map->ds_map_csum = to.to_mopts.dss_csum;
+
+				mp_debug(DSMAP, 4, 0, "%s: inserting map sf_seq_start %u, "
+					"end %u seq: %u\n", __func__, map->sf_seq_start,
+					map->sf_seq_start + map->ds_map_len, th->th_seq);
+
+				/* if there are no existing maps insert at head. Other
+				 * wise insert in sequence */
+				if (next_map == NULL) {
+//					KASSERT(TAILQ_EMPTY(&tp->t_rxdsmaps),
+//						("Incoming sequence %u less than those in rx maps\n"));
+//					TAILQ_INSERT_HEAD(&tp->t_rxdsmaps, map, sf_ds_map_next);
+					if (TAILQ_EMPTY(&tp->t_rxdsmaps) ||
+						map->sf_seq_start == tp->rcv_nxt) {
+						TAILQ_INSERT_TAIL(&tp->t_rxdsmaps, map, sf_ds_map_next);
+					} else {
+						goto dropafterack;
+					}
+				} else {
+					TAILQ_INSERT_BEFORE(next_map, map, sf_ds_map_next);
+				}
+
+				///* Walk list of unmapped packets and call tcp_do_segment for
+				 //* segments that belong to the newly created ds_map entry */
+				//uint32_t map_end = map->sf_seq_start + map->ds_map_len;
+				//struct tseg_qent *u_seg, *temp_seg;
+				//TAILQ_FOREACH_SAFE(u_seg, tp->t_unmapped_segq, tqe_q, temp_seg) {
+					//uint32_t seqnum = u_seg->tqe_th->th_seq;
+					//if ((seqnum >= map->sf_seq_start && map_end > map->sf_seq_start
+						//&& seqnum < map_end) ||
+						//(map_end < map->sf_seq_start && seqnum < map->sf_seq_start
+						//&& seqnum < map_end) ||
+						//(map_end < map->sf_seq_start && seqnum >= map->sf_seq_start
+						//&& seqnum <= UINT_MAX)) {
+							//mp_debug(DSMAP, 1, 0, "found map for unmapped "
+							//"segment: ds_map_start %ju, sd_seq_start: %u. len: "
+							//"%d\n", map->ds_map_start, map->sf_seq_start,
+							//map->ds_map_len);
+
+							//printf("found map for unmapped "
+							//"segment: map_sf_seq_start: %u. len: "
+							//"%d, seqn: %u\n", map->sf_seq_start,
+							//map->ds_map_len, seqnum);
+
+							///*
+							 //* We have the map for this packet. call
+							 //* tcp_do_segment then remove from list.
+							 //* This will process the segment as would have
+							 //* happened if it arrived after the map.
+							 //*
+							 //* XXXNJW:
+							 //* Ideally we should process the just-arrived seg
+							 //* first, so that these buffered segments do have
+							 //* to wait in reassembly.
+							 //*/
+							//INP_WLOCK_ASSERT(tp->t_inpcb);
+							//tcp_do_segment(u_seg->tqe_m, u_seg->tqe_th, so, tp,
+							    //drop_hdrlen, u_seg->tqe_len,
+							    //iptos, ti_locked);
+							//TAILQ_REMOVE(tp->t_unmapped_segq, u_seg, tqe_q);
+							//uma_zfree(V_tcp_unmapped_zone, u_seg);
+							//continue;
+					//}
+				//}
+			}
+			/*
+			 * If in direct mapping mode and have received a DSS, then
+			 * change ds_rcv_nxt to the incoming DSN. We are no longer
+			 * in direct map mode at this point.
+			 * XXXNJW: Shared data access
+			 */
+			if (tp->infinite_map) {
+				tp->t_mpcb->ds_rcv_nxt = to.to_mopts.data_seq_num;
+				tp->infinite_map = 0;
+			}
+		}
+	} /* End DSS Map processing */
+
+	/*
+	 * Processing ADD_ADDR options. This will add the new remote address
+	 * to the path manager, making it available for this mp connection.
+	 * Usually a server might advertise additional address that a client
+	 * can connect to.
+	 */
+	if (to.to_mopts.mpo_flags & (MPOF_ADD_ADDR_V4 | MPOF_ADD_ADDR_V6)) {
+		/* insert into list of usable addresses at index AddrID */
+		/* store the in_addr in the sockaddr_storage */
+
+		bcopy(&to.to_mopts.new_addr,
+		    &mp->mp_added_addresses[to.to_mopts.addr_id],
+		    sizeof(struct sockaddr_storage));
+
+		char buf[128];
+		inet_ntop(AF_INET, &(((struct sockaddr_in *)
+		    &mp->mp_added_addresses[to.to_mopts.addr_id])->sin_addr.s_addr),
+		    buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "added address %s\n", buf);
+
+		inet_ntop(AF_INET, &(((struct sockaddr_in *)
+			&to.to_mopts.new_addr)->sin_addr.s_addr),
+			buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "got address %s\n", buf);
+
+		/* XXXNJW: This is a hack in lieu of a proper path manager. prevents
+		 * a multi-homed host from  from attempting to MP_JOIN to any
+		 * addresses it has received via ADD_ADDR */
+		if (tp->t_mpcb->mp_conn_address_count == 1 ||
+		      (tp->t_mpcb->mp_conn_address_count > 1
+			      && !tp->t_mpcb->passive_open) ) {
+			/*
+			 * we have a new address to attempt to join. Increment the advjoin
+			 * and address count, schedule a task to perform a join
+			 * We increment added_address_count _after_ the bitshift so that
+			 * the shift is by the correct amount
+			 */
+			tp->t_mpcb->mp_advjoin_mask |=
+			    (1 << (tp->t_mpcb->mp_added_address_count));
+			atomic_add_int(&tp->t_mpcb->mp_added_address_count,1);
+			mp_debug(MPSESSION, 4, 0, "%s: advjoinmask: %u\n", __func__,
+			    tp->t_mpcb->mp_advjoin_mask);
+		} else {
+
+		}
+	}
+
+	if (tlen) {
+		/*
+		 * Tag the mbuf with data-level sequence number. A direct
+		 * map will map the tcp sequence number directly to the
+		 * data sequence number. Otherwise calculate the dsn and
+		 * tag the mbuf with this value.
+		 *
+		 * XXXNJW: mtag might not be required, see tcp_reass code.
+		 */
+		struct dsn_tag *dtag = (struct dsn_tag *)
+					m_tag_alloc(PACKET_COOKIE_MPTCP,
+					PACKET_TAG_DSN, DSN_LEN, M_NOWAIT); // space for 64-bit DSN
+		struct m_tag *mtag = &dtag->tag;
+
+		//XXXNJW: shared data access
+		if (tp->infinite_map) {
+			dtag->dsn = th->th_seq;
+		} else {
+			if (map == NULL) {
+				/* find the map that this segment belongs to. First map that
+				 * incoming subflow seq num is larger than */
+				TAILQ_FOREACH(map, &tp->t_rxdsmaps, sf_ds_map_next) {
+					/*
+					 * XXXNJW: The following panic needs to be changed to handle
+					 * the buffering of packets beyond the mapped area (for cases where
+					 * the DSN-carrying packet has been lost, or arrives out of order.
+					 *
+					 * XXXNJW: Collapse this into a single block once verified
+					 */
+//					if (map->sf_seq_start + map->ds_map_len > map->sf_seq_start) {
+//						if (th->th_seq > map->sf_seq_start + map->ds_map_len)
+//							panic("%s: Received data beyond end of mapped region."
+//							"ssn: %u, mapstart %u, maplen %u\n",
+//							__func__, th->th_seq, map->sf_seq_start, map->ds_map_len);
+//					} else {
+//						if ((th->th_seq > map->sf_seq_start + map->ds_map_len) &&
+//						    (th->th_seq < map->sf_seq_start))
+//							panic("%s: Received data beyond end of mapped region."
+//							"ssn: %u, mapstart %u, maplen %u\n",
+//							__func__, th->th_seq, map->sf_seq_start, map->ds_map_len);
+//					}
+
+					uint32_t seqnum = th->th_seq;
+					uint32_t sf_seq_end = map->sf_seq_start + map->ds_map_len;
+					if ((seqnum >= map->sf_seq_start && sf_seq_end > map->sf_seq_start
+						&& seqnum < sf_seq_end) ||
+						(sf_seq_end < map->sf_seq_start && seqnum < map->sf_seq_start
+						&& seqnum < sf_seq_end) ||
+						(sf_seq_end < map->sf_seq_start && seqnum >= map->sf_seq_start
+						&& seqnum <= UINT_MAX)) {
+							/* A map exists for this packet */
+							mp_debug(DSMAP, 4, 0, "found map: ds_map_start %ju, "
+							"sd_seq_start: %u. len: %d\n", map->ds_map_start,
+							map->sf_seq_start, map->ds_map_len);
+							break;
+					}
+				}
+
+			}
+
+			/* Must have a valid map beyond this point */
+//			KASSERT(map != NULL, ("%s:%d No map found for packet with "
+//			    "length %d, mp_active is: %d: ssn: %u\n",
+//			    __func__, __LINE__, tlen, tp->mp_active, th->th_seq));
+
+			/*
+			 * In multi-packet map scenario, the packet carrying the map
+			 * might be lost. Thus packets arriving after this will not
+			 * belong to an existing map. These packets are buffered while
+			 * we wait for the packet with the map to be retransmitted.
+			 * If the zone for buffering packets is exhausted, we simply send
+			 * and ACK and drop the segment, making the sender retransmit.
+			 * Otherwise we insert the segment into the list.
+			 */
+			if(map == NULL) {
+				mp_debug(DSMAP, 4, 0,"%s: no map, dropping segment %u dsn: %u\n"
+				    , __func__, th->th_seq, (uint32_t) to.to_mopts.data_seq_num);
+				struct tseg_qent *te = NULL;
+				te = uma_zalloc(V_tcp_unmapped_zone, M_NOWAIT);
+				if (te == NULL) {
+						goto dropafterack;
+				}
+
+				/* XXXNJW not yet handling unmapped packets */
+				goto dropafterack;
+
+				te->tqe_m = m;
+				te->tqe_th = th;
+				te->tqe_len = tlen;
+				te->tqe_dsn = 0;
+
+				/* Push packet into unmapped list and trigger ACK */
+				TAILQ_INSERT_HEAD(tp->t_unmapped_segq, te, tqe_q);
+				tp->t_flags |= TF_ACKNOW;
+				tcp_output(tp);
+				goto check_delack;
+			}
+
+			/* update mbuf tag with current data seq num */
+			dtag->dsn = map->ds_map_start + (th->th_seq - map->sf_seq_start);
+		}
+
+		/* And prepend the mtag to mbuf, to be checked in reass */
+		m_tag_prepend(m, mtag);
+	} /* End tlen > 0 */
+
+	/*
 	 * Process options only when we get SYN/ACK back. The SYN case
 	 * for incoming connections is handled in tcp_syncache.
 	 * According to RFC1323 the window field in a SYN (i.e., a <SYN>
 	 * or <SYN,ACK>) segment itself is never scaled.
-	 * XXX this is traditional behavior, may need to be cleaned up.
+	 * XXX this is traditional behaviour, may need to be cleaned up.
+	 */
+
+	/*
+	 * XXXNJW: There is some access to shared mpcb structure in here.
 	 */
 	if (tp->t_state == TCPS_SYN_SENT && (thflags & TH_SYN)) {
 		if ((to.to_flags & TOF_SCALE) &&
@@ -1614,7 +2505,69 @@
 		if ((tp->t_flags & TF_SACK_PERMIT) &&
 		    (to.to_flags & TOF_SACKPERM) == 0)
 			tp->t_flags &= ~TF_SACK_PERMIT;
-	}
+
+		if (to.to_flags & TOF_MPTCP) {
+			if (IS_MASTER_TP(tp)) {
+				if (tp->sent_capable) {
+					mp_process_remote_key(tp->t_mpcb, to.to_mopts.remote_key);
+
+					/* send mp_cabable ACK, got SYN/ACK. NOTE this is
+					 * different to JOIN_SYNACK */
+					tp->sf_flags |= SFF_GOT_SYNACK;
+					/* we know the other end is mp enabled */
+					tp->mpc = 1;
+					/* enable csum if remote end sets csum bit */
+					if (to.to_mopts.mpo_flags & MPOF_USE_CSUM)
+						tp->t_mpcb->csum_enabled = 1;
+				} else {
+					/* got a MP_CAPABLE but did not send this option */
+					tp->mpc = 1;
+					tp->mp_active = 0;
+				}
+			} else {
+				/* could be a subflow SYN/ACK */
+				if (to.to_mopts.mpo_flags & MPOF_JOIN_SYNACK) {
+					if (to.to_mopts.mpo_flags & MPOF_BACKUP_PATH) {
+						// XXXNJW tbd: this flow is a backup path
+						// should drop the subflow rather than panic the
+						// host
+						panic("Backup subflows not supported yet\n");
+					}
+
+					/* mp is available on this flow */
+					tp->mpc = 1;
+
+					tp->hmac_trun_remote = to.to_mopts.snd_trc_mac;
+					tp->remote_rand = to.to_mopts.snd_rnd;
+
+					/* Calculate a MAC to return in the MP_JOIN ACK using
+					 * the random number just rcvd, and the keys from the
+					 * initial mp establishment
+					 */
+
+					/* MAC-A */
+					uint32_t mac_len = mp_get_hmac(tp->hmac_local,
+					    mp->local_key, mp->remote_key, tp->local_rand,
+					    tp->remote_rand);
+
+					if (mac_len == 0) {
+						mp_debug(MPSESSION, 4, 0,"%s: error computing MAC-A\n",
+						    __func__);
+						goto dropwithreset;
+					}
+
+					// XXXNJW: Need to validate the HMACs before continuing
+					// i.e. calculate the remote hmac and compare the leftmost
+					// 64 bits with the those in the truncated HMAC
+
+					tp->sf_flags |= SFF_GOT_JOIN_SYNACK;
+					mp_debug(MPSESSION, 4, 0,"%s: got join syn/ack\n",
+					    __func__);
+				}
+			}
+		}
+
+	} /* End SYN/ACK processing */
 
 	/*
 	 * Header prediction: check for the two common cases
@@ -1639,7 +2592,7 @@
 	    tp->snd_nxt == tp->snd_max &&
 	    tiwin && tiwin == tp->snd_wnd && 
 	    ((tp->t_flags & (TF_NEEDSYN|TF_NEEDFIN)) == 0) &&
-	    LIST_EMPTY(&tp->t_segq) &&
+	    !tp->t_segq->disordered &&									// XXXNJW: check disordered
 	    ((to.to_flags & TOF_TS) == 0 ||
 	     TSTMP_GEQ(to.to_tsval, tp->ts_recent)) ) {
 
@@ -1704,27 +2657,44 @@
 					tcp_xmit_timer(tp,
 							ticks - tp->t_rtttime);
 				}
-				acked = BYTES_THIS_ACK(tp, th);
 
 				/* Run HHOOK_TCP_ESTABLISHED_IN helper hooks. */
 				hhook_run_tcp_est_in(tp, th, &to);
 
 				TCPSTAT_INC(tcps_rcvackpack);
-				TCPSTAT_ADD(tcps_rcvackbyte, acked);
-				sbdrop(&so->so_snd, acked);
+				TCPSTAT_ADD(tcps_rcvackbyte, acked); // pure ack for outstanding
 				if (SEQ_GT(tp->snd_una, tp->snd_recover) &&
 				    SEQ_LEQ(th->th_ack, tp->snd_recover))
 					tp->snd_recover = th->th_ack - 1;
 				
+				mp_debug(MPSESSION, 3, 0, "%s: sb_cc: %u sb_actual: %u th_ack: "
+						"%u, snd_una: %u acked: %d: \n",
+					__func__, so->so_snd.sb_cc, so->so_snd.sb_actual,
+					th->th_ack, tp->snd_una, acked);
+
 				/*
 				 * Let the congestion control algorithm update
 				 * congestion control related information. This
 				 * typically means increasing the congestion
 				 * window.
 				 */
-				cc_ack_received(tp, th, CC_ACK);
+				cc_ack_received(tp, th, CC_ACK, acked);
+
+				/* Can't have an ACK of a FIN here, so do not need to adjust
+				 * by one byte here.
+				 * XXX: This is also a temporary hack and should be changed
+				 */
+				if (tp->infinite_map) {
+					KASSERT(acked <=  so->so_snd.sb_cc,
+					    ("%s:%d: sb_cc: %d, acked: %d", __func__, __LINE__,
+					    so->so_snd.sb_cc, acked));
+					    atomic_subtract_int(&so->so_snd.sb_cc, acked);
+				}
+				tcp_ackmap(tp, acked, (to.to_mopts.mpo_flags & MPOF_DATA_ACK)
+				    ? to.to_mopts.data_ack_num : 0);
 
 				tp->snd_una = th->th_ack;
+
 				/*
 				 * Pull snd_wl2 up to prevent seq wrap relative
 				 * to th_ack.
@@ -1754,14 +2724,18 @@
 				else if (!tcp_timer_active(tp, TT_PERSIST))
 					tcp_timer_activate(tp, TT_REXMT,
 						      tp->t_rxtcur);
-				sowwakeup(so);
-				if (so->so_snd.sb_cc)
+
+				/* XXXNJW: macro for the first condition below (i.e. unmapped data in sb) */
+				if (SB_NUNMAPPED(&so->so_snd, mp) > 0
+				    || (TAILQ_FIRST(&tp->t_txdsmaps) != NULL &&
+				    !(TAILQ_FIRST(&tp->t_txdsmaps)->flags & MAPF_IS_SENT)))
 					(void) tcp_output(tp);
 				goto check_delack;
 			}
+			/* End pure ACK processing */
 		} else if (th->th_ack == tp->snd_una &&
-		    tlen <= sbspace(&so->so_rcv)) {
-			int newsize = 0;	/* automatic sockbuf scaling */
+		    tlen <= sbspace(&so->so_rcv)) { //XXXNJW okay to base off of sb_cc/ sb_max, sb_mcnt?
+//			int newsize = 0;	/* automatic sockbuf scaling */
 
 			/*
 			 * This is a pure, in-sequence data packet with
@@ -1776,7 +2750,6 @@
 			if ((tp->t_flags & TF_SACK_PERMIT) && tp->rcv_numsacks)
 				tcp_clean_sackreport(tp);
 			TCPSTAT_INC(tcps_preddat);
-			tp->rcv_nxt += tlen;
 			/*
 			 * Pull snd_wl1 up to prevent seq wrap relative to
 			 * th_seq.
@@ -1789,7 +2762,7 @@
 			tp->rcv_up = tp->rcv_nxt;
 			TCPSTAT_INC(tcps_rcvpack);
 			TCPSTAT_ADD(tcps_rcvbyte, tlen);
-			ND6_HINT(tp);	/* Some progress has been made */
+			//ND6_HINT(tp);	/* Some progress has been made */
 #ifdef TCPDEBUG
 			if (so->so_options & SO_DEBUG)
 				tcp_trace(TA_INPUT, ostate, tp,
@@ -1828,45 +2801,58 @@
 		 * TODO: Only step up if the application is actually serving
 		 * the buffer to better manage the socket buffer resources.
 		 */
-			if (V_tcp_do_autorcvbuf &&
-			    to.to_tsecr &&
-			    (so->so_rcv.sb_flags & SB_AUTOSIZE)) {
-				if (TSTMP_GT(to.to_tsecr, tp->rfbuf_ts) &&
-				    to.to_tsecr - tp->rfbuf_ts < hz) {
-					if (tp->rfbuf_cnt >
-					    (so->so_rcv.sb_hiwat / 8 * 7) &&
-					    so->so_rcv.sb_hiwat <
-					    V_tcp_autorcvbuf_max) {
-						newsize =
-						    min(so->so_rcv.sb_hiwat +
-						    V_tcp_autorcvbuf_inc,
-						    V_tcp_autorcvbuf_max);
-					}
-					/* Start over with next RTT. */
-					tp->rfbuf_ts = 0;
-					tp->rfbuf_cnt = 0;
-				} else
-					tp->rfbuf_cnt += tlen;	/* add up */
-			}
+
+			/*
+			 * XXXNJW: Auto buffer tuning may not work as we no longer store
+			 * things in the socket buffers
+			 */
+//			if (V_tcp_do_autorcvbuf &&
+//			    to.to_tsecr &&
+//			    (so->so_rcv.sb_flags & SB_AUTOSIZE)) {
+//				if (TSTMP_GT(to.to_tsecr, tp->rfbuf_ts) &&
+//				    to.to_tsecr - tp->rfbuf_ts < hz) {
+//					if (tp->rfbuf_cnt >
+//					    (so->so_rcv.sb_hiwat / 8 * 7) &&
+//					    so->so_rcv.sb_hiwat <
+//					    V_tcp_autorcvbuf_max) {
+//						newsize =
+//						    min(so->so_rcv.sb_hiwat +
+//						    V_tcp_autorcvbuf_inc,
+//						    V_tcp_autorcvbuf_max);
+//					}
+//					/* Start over with next RTT. */
+//					tp->rfbuf_ts = 0;
+//					tp->rfbuf_cnt = 0;
+//				} else
+//					tp->rfbuf_cnt += tlen;	/* add up */
+//			}
+
+			/* XXXNJW leaving auto scaling code in as need to revisit the
+			 * rcv buffer at some point */
 
 			/* Add data to socket buffer. */
-			SOCKBUF_LOCK(&so->so_rcv);
+			//SOCKBUF_LOCK(&so->so_rcv);
 			if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
 				m_freem(m);
+				//SOCKBUF_UNLOCK(&so->so_rcv);
 			} else {
 				/*
 				 * Set new socket buffer size.
 				 * Give up when limit is reached.
 				 */
-				if (newsize)
-					if (!sbreserve_locked(&so->so_rcv,
-					    newsize, so, NULL))
-						so->so_rcv.sb_flags &= ~SB_AUTOSIZE;
+//				if (newsize)
+//					if (!sbreserve_locked(&so->so_rcv,
+//					    newsize, so, NULL))
+//						so->so_rcv.sb_flags &= ~SB_AUTOSIZE;
 				m_adj(m, drop_hdrlen);	/* delayed header drop */
-				sbappendstream_locked(&so->so_rcv, m);
+
+				/*
+				 * tcp_reass will aquire lock only if it needs to do
+				 * reassembly.
+				 */
+				//SOCKBUF_UNLOCK(&so->so_rcv);
+				thflags = tcp_reass(tp, th, &tlen, m);
 			}
-			/* NB: sorwakeup_locked() does an implicit unlock. */
-			sorwakeup_locked(so);
 			if (DELAY_ACK(tp, tlen)) {
 				tp->t_flags |= TF_DELACK;
 			} else {
@@ -1875,17 +2861,28 @@
 			}
 			goto check_delack;
 		}
-	}
+	} /* End fast path (TCPS_ESTABLISHED) */
 
 	/*
 	 * Calculate amount of space in receive window,
 	 * and then do TCP input processing.
 	 * Receive window is amount of space in rcv queue,
 	 * but not less than advertised window.
+	 *
+	 * XXXNJW: When mptcp is active, want to use the
+	 * connection-level receive window, stored in the
+	 * mpcb.
 	 */
-	win = sbspace(&so->so_rcv);
+	//win = sbspace(&so->so_rcv);
+
+//	if (tp->mp_active)
+		win = mp_get_recwin(tp->t_mpcb);
+//	else
+//		win = sbspace(&so->so_rcv);
+
 	if (win < 0)
-		win = 0;
+        win = 0;
+
 	tp->rcv_wnd = imax(win, (int)(tp->rcv_adv - tp->rcv_nxt));
 
 	/* Reset receive buffer auto scaling when not in bulk receive mode. */
@@ -1896,7 +2893,7 @@
 
 	/*
 	 * If the state is SYN_RECEIVED:
-	 *	if seg contains an ACK, but not for our SYN/ACK, send a RST.
+	 * if seg contains an ACK, but not for our SYN/ACK, send a RST.
 	 */
 	case TCPS_SYN_RECEIVED:
 		if ((thflags & TH_ACK) &&
@@ -1931,14 +2928,21 @@
 		if ((thflags & (TH_ACK|TH_RST)) == (TH_ACK|TH_RST)) {
 			TCP_PROBE5(connect__refused, NULL, tp,
 			    mtod(m, const char *), tp, th);
-			tp = tcp_drop(tp, ECONNREFUSED);
+			if (IS_MASTER_TP(tp))	//workaround to prevent failed JOINs killing the connection
+			    tp = tcp_drop(tp, ECONNREFUSED);
 		}
 		if (thflags & TH_RST)
 			goto drop;
 		if (!(thflags & TH_SYN))
 			goto drop;
 
+		/* Set initial receive values for tpcb and mpcb. initially we
+		 * are in infinite mapping mode so both key off tp->irs.
+		 * mp protocol specifies increment by 1, for some reason. */
 		tp->irs = th->th_seq;
+		if (IS_MASTER_TP(tp))
+			tp->t_mpcb->ds_rcv_nxt = tp->irs + 1;
+
 		tcp_rcvseqinit(tp);
 		if (thflags & TH_ACK) {
 			TCPSTAT_INC(tcps_connects);
@@ -1954,6 +2958,7 @@
 			tp->rcv_adv += imin(tp->rcv_wnd,
 			    TCP_MAXWIN << tp->rcv_scale);
 			tp->snd_una++;		/* SYN is acked */
+
 			/*
 			 * If there's data, delay ACK; if there's also a FIN
 			 * ACKNOW will be turned on later.
@@ -1968,7 +2973,7 @@
 				tp->t_flags |= TF_ECN_PERMIT;
 				TCPSTAT_INC(tcps_ecn_shs);
 			}
-			
+
 			/*
 			 * Received <SYN,ACK> in SYN_SENT[*] state.
 			 * Transitions:
@@ -1987,6 +2992,19 @@
 				cc_conn_init(tp);
 				tcp_timer_activate(tp, TT_KEEP,
 				    TP_KEEPIDLE(tp));
+				if (to.to_flags & TOF_MPTCP) {
+					if (tp->sent_capable) {
+						/* active open */
+
+						/*  XXXNJW shared access. might be okay as only one
+						 * subflow will exist at this point? (this would make
+						 * the below check irrelevent though... */
+
+						/* Don't want a new subflow to switch the mp state */
+						if(IS_MASTER_TP(tp))
+							mp_init_established(tp);
+					}
+				}
 			}
 		} else {
 			/*
@@ -2035,6 +3053,8 @@
 
 		goto step6;
 
+	/* Case SYN_SENT */
+
 	/*
 	 * If the state is LAST_ACK or CLOSING or TIME_WAIT:
 	 *      do normal processing.
@@ -2045,6 +3065,8 @@
 	case TCPS_CLOSING:
 		break;  /* continue normal processing */
 	}
+	/* End SYN/LISTEN processing */
+
 
 	/*
 	 * States other than LISTEN or SYN_SENT.
@@ -2155,6 +3177,8 @@
 		}
 		goto drop;
 	}
+	/* End RST Processing */
+
 
 	/*
 	 * RFC 1323 PAWS: If we have a timestamp reply on this segment
@@ -2199,6 +3223,7 @@
 		goto dropwithreset;
 	}
 
+	/* Trimming to fit rcv window */
 	todrop = tp->rcv_nxt - th->th_seq;
 	if (todrop > 0) {
 		/*
@@ -2238,16 +3263,27 @@
 			TCPSTAT_INC(tcps_rcvpartduppack);
 			TCPSTAT_ADD(tcps_rcvpartdupbyte, todrop);
 		}
+
 		drop_hdrlen += todrop;	/* drop from the top afterwards */
 		th->th_seq += todrop;
 		tlen -= todrop;
+
+		/* dsn should be incremented by the same length as th_seq */
+		if (map) {
+			struct m_tag *mtag =  m_tag_locate(m, PACKET_COOKIE_MPTCP,
+			    PACKET_TAG_DSN, NULL);
+			KASSERT(mtag != NULL, ("%s segment %u missing an mbuf tag\n",
+			    __func__, th->th_seq - todrop));
+			((struct dsn_tag *)mtag)->dsn += todrop;
+		}
+
 		if (th->th_urp > todrop)
 			th->th_urp -= todrop;
 		else {
 			thflags &= ~TH_URG;
 			th->th_urp = 0;
 		}
-	}
+	} /* end segment trimming, seq < rcv_nxt */
 
 	/*
 	 * If new data are received on a connection after the
@@ -2278,6 +3314,10 @@
 	 */
 	todrop = (th->th_seq + tlen) - (tp->rcv_nxt + tp->rcv_wnd);
 	if (todrop > 0) {
+//		printf("%s: tseq: %u trim %d, window %ld mp %p\n", __func__,
+//		    th->th_seq, todrop, tp->rcv_wnd, tp->t_mpcb);
+//		printf("mp %p tp %p\n", mp, tp);
+//		kdb_break();
 		TCPSTAT_INC(tcps_rcvpackafterwin);
 		if (todrop >= tlen) {
 			TCPSTAT_ADD(tcps_rcvbyteafterwin, tlen);
@@ -2295,6 +3335,7 @@
 				goto dropafterack;
 		} else
 			TCPSTAT_ADD(tcps_rcvbyteafterwin, todrop);
+																				goto dropafterack; //XXXNJW cheap hack test
 		m_adj(m, -todrop);
 		tlen -= todrop;
 		thflags &= ~(TH_PUSH|TH_FIN);
@@ -2373,13 +3414,23 @@
 		if ((tp->t_flags & (TF_RCVD_SCALE|TF_REQ_SCALE)) ==
 			(TF_RCVD_SCALE|TF_REQ_SCALE)) {
 			tp->rcv_scale = tp->request_r_scale;
-			tp->snd_wnd = tiwin;
+
+			if(IS_MASTER_TP(tp)) {
+				tp->snd_wnd = tiwin;
+				// store the unscaled value in mpcb
+				tp->t_mpcb->ds_snd_wnd = (uint32_t) (tiwin >> tp->snd_scale);
+			} else {
+				// set it to the existing window for the connection
+				tp->snd_wnd = tp->t_mpcb->ds_snd_wnd;
+			}
 		}
 		/*
 		 * Make transitions:
 		 *      SYN-RECEIVED  -> ESTABLISHED
 		 *      SYN-RECEIVED* -> FIN-WAIT-1
 		 */
+		oursynisacked = 1;
+
 		tp->t_starttime = ticks;
 		if (tp->t_flags & TF_NEEDFIN) {
 			tcp_state_change(tp, TCPS_FIN_WAIT_1);
@@ -2390,6 +3441,12 @@
 			    mtod(m, const char *), tp, th);
 			cc_conn_init(tp);
 			tcp_timer_activate(tp, TT_KEEP, TP_KEEPIDLE(tp));
+			if (to.to_flags & TOF_MPTCP) {
+				/* passive open */
+				if(tp->mpc && tp->sent_capable) {
+					mp_init_established(tp);
+				}
+			}
 		}
 		/*
 		 * If segment contains data or ACK, will call tcp_reass()
@@ -2410,6 +3467,17 @@
 	 * more up to date window information we update our window information.
 	 */
 	case TCPS_ESTABLISHED:
+		if(!IS_MASTER_TP(tp) && tp->sf_state == MPS_PRE_ESTABLISHED) {
+			tp->mp_active = 1;
+			tp->infinite_map = 0;
+			tp->sf_state = MPS_ESTABLISHED;
+			mp_debug(MPSESSION, 1, 0, "%s:%d new mp connection for tp: %p: \n",
+                __func__, __LINE__, tp);
+//			atomic_add_int(&tp->t_mpcb->subflow_cnt,1);
+			mp_debug(MPSESSION, 4, 0, "%s: set subflow as mp_active\n",
+			    __func__);
+		}
+		/* drop through */
 	case TCPS_FIN_WAIT_1:
 	case TCPS_FIN_WAIT_2:
 	case TCPS_CLOSE_WAIT:
@@ -2441,6 +3509,7 @@
 					tp->t_dupacks = 0;
 					break;
 				}
+
 				TCPSTAT_INC(tcps_rcvdupack);
 				/*
 				 * If we have outstanding data (other than
@@ -2475,14 +3544,14 @@
 					tp->t_dupacks = 0;
 				else if (++tp->t_dupacks > tcprexmtthresh ||
 				     IN_FASTRECOVERY(tp->t_flags)) {
-					cc_ack_received(tp, th, CC_DUPACK);
+					cc_ack_received(tp, th, CC_DUPACK, acked);
 					if ((tp->t_flags & TF_SACK_PERMIT) &&
 					    IN_FASTRECOVERY(tp->t_flags)) {
 						int awnd;
 						
 						/*
 						 * Compute the amount of data in flight first.
-						 * We can inject new data into the pipe iff 
+						 * We can inject new data into the pipe if
 						 * we have less than 1/2 the original window's
 						 * worth of data in flight.
 						 */
@@ -2493,13 +3562,16 @@
 							if (tp->snd_cwnd > tp->snd_ssthresh)
 								tp->snd_cwnd = tp->snd_ssthresh;
 						}
+						if (tp->t_dupacks == (tcprexmtthresh +1))
+							mp_debug(MPSESSION, 4, 0,
+							    "%s: exceeded rxthresh. awnd %d, cwnd %u\n",
+								__func__, awnd, (uint32_t) tp->snd_cwnd);
 					} else
 						tp->snd_cwnd += tp->t_maxseg;
 					(void) tcp_output(tp);
 					goto drop;
 				} else if (tp->t_dupacks == tcprexmtthresh) {
 					tcp_seq onxt = tp->snd_nxt;
-
 					/*
 					 * If we're doing sack, check to
 					 * see if we're already in sack
@@ -2521,9 +3593,10 @@
 					}
 					/* Congestion signal before ack. */
 					cc_cong_signal(tp, th, CC_NDUPACK);
-					cc_ack_received(tp, th, CC_DUPACK);
+					cc_ack_received(tp, th, CC_DUPACK, acked);
 					tcp_timer_activate(tp, TT_REXMT, 0);
 					tp->t_rtttime = 0;
+					tp->snd_nxt = th->th_ack;
 					if (tp->t_flags & TF_SACK_PERMIT) {
 						TCPSTAT_INC(
 						    tcps_sack_recovery_episode);
@@ -2532,7 +3605,6 @@
 						(void) tcp_output(tp);
 						goto drop;
 					}
-					tp->snd_nxt = th->th_ack;
 					tp->snd_cwnd = tp->t_maxseg;
 					(void) tcp_output(tp);
 					KASSERT(tp->snd_limited <= 2,
@@ -2545,7 +3617,7 @@
 						tp->snd_nxt = onxt;
 					goto drop;
 				} else if (V_tcp_do_rfc3042) {
-					cc_ack_received(tp, th, CC_DUPACK);
+					cc_ack_received(tp, th, CC_DUPACK, acked);
 					u_long oldcwnd = tp->snd_cwnd;
 					tcp_seq oldsndmax = tp->snd_max;
 					u_int sent;
@@ -2574,12 +3646,12 @@
 						(void) tcp_output(tp);
 					sent = tp->snd_max - oldsndmax;
 					if (sent > tp->t_maxseg) {
-						KASSERT((tp->t_dupacks == 2 &&
-						    tp->snd_limited == 0) ||
-						   (sent == tp->t_maxseg + 1 &&
-						    tp->t_flags & TF_SENTFIN),
-						    ("%s: sent too much",
-						    __func__));
+//						KASSERT((tp->t_dupacks == 2 &&
+//						    tp->snd_limited == 0) ||
+//						   (sent == tp->t_maxseg + 1 &&
+//						    tp->t_flags & TF_SENTFIN),
+//						    ("%s: sent too much",
+//						    __func__));
 						tp->snd_limited = 2;
 					} else if (sent > 0)
 						++tp->snd_limited;
@@ -2589,7 +3661,7 @@
 			} else
 				tp->t_dupacks = 0;
 			break;
-		}
+		} /* end dup-ack processing */
 
 		KASSERT(SEQ_GT(th->th_ack, tp->snd_una),
 		    ("%s: th_ack <= snd_una", __func__));
@@ -2601,9 +3673,9 @@
 		if (IN_FASTRECOVERY(tp->t_flags)) {
 			if (SEQ_LT(th->th_ack, tp->snd_recover)) {
 				if (tp->t_flags & TF_SACK_PERMIT)
-					tcp_sack_partialack(tp, th);
+					tcp_sack_partialack(tp, th, acked);
 				else
-					tcp_newreno_partial_ack(tp, th);
+					tcp_newreno_partial_ack(tp, th, acked);
 			} else
 				cc_post_recovery(tp, th);
 		}
@@ -2622,6 +3694,7 @@
 			 */
 			tp->t_flags &= ~TF_NEEDSYN;
 			tp->snd_una++;
+
 			/* Do window scaling? */
 			if ((tp->t_flags & (TF_RCVD_SCALE|TF_REQ_SCALE)) ==
 				(TF_RCVD_SCALE|TF_REQ_SCALE)) {
@@ -2633,7 +3706,6 @@
 process_ACK:
 		INP_WLOCK_ASSERT(tp->t_inpcb);
 
-		acked = BYTES_THIS_ACK(tp, th);
 		TCPSTAT_INC(tcps_rcvackpack);
 		TCPSTAT_ADD(tcps_rcvackbyte, acked);
 
@@ -2688,33 +3760,29 @@
 			tcp_timer_activate(tp, TT_REXMT, tp->t_rxtcur);
 
 		/*
-		 * If no data (only SYN) was ACK'd,
-		 *    skip rest of ACK processing.
+		 * If no data (only SYN or FIN) was ACK'd,
+		 * skip rest of ACK processing.
 		 */
-		if (acked == 0)
+		if (acked == 0 && (FAKE_BYTES_THIS_ACK(tp, th) == 0))
 			goto step6;
 
+		acked -= (oursynisacked ? 1 : 0);
+
 		/*
 		 * Let the congestion control algorithm update congestion
 		 * control related information. This typically means increasing
 		 * the congestion window.
 		 */
-		cc_ack_received(tp, th, CC_ACK);
-
-		SOCKBUF_LOCK(&so->so_snd);
-		if (acked > so->so_snd.sb_cc) {
-			tp->snd_wnd -= so->so_snd.sb_cc;
-			mfree = sbcut_locked(&so->so_snd,
-			    (int)so->so_snd.sb_cc);
+		cc_ack_received(tp, th, CC_ACK, acked);
+
+		if ((tp->t_flags & TF_SENTFIN) && (th->th_ack == tp->snd_max)) {
+			tp->snd_wnd -= ulmin(tp->snd_wnd, acked);
 			ourfinisacked = 1;
 		} else {
-			mfree = sbcut_locked(&so->so_snd, acked);
 			tp->snd_wnd -= acked;
 			ourfinisacked = 0;
 		}
-		/* NB: sowwakeup_locked() does an implicit unlock. */
-		sowwakeup_locked(so);
-		m_freem(mfree);
+		
 		/* Detect una wraparound. */
 		if (!IN_RECOVERY(tp->t_flags) &&
 		    SEQ_GT(tp->snd_una, tp->snd_recover) &&
@@ -2725,7 +3793,18 @@
 		    SEQ_GEQ(th->th_ack, tp->snd_recover)) {
 			EXIT_RECOVERY(tp->t_flags);
 		}
+
+		/* XXXNJW to be changed */
+		if (tp->infinite_map) {
+			KASSERT(acked <=  so->so_snd.sb_cc,
+			    ("sb_cc: %d, acked: %d", so->so_snd.sb_cc, acked));
+			atomic_subtract_int(&so->so_snd.sb_cc, acked);
+		}
+		tcp_ackmap(tp, acked, (to.to_mopts.mpo_flags & MPOF_DATA_ACK) ?
+		    to.to_mopts.data_ack_num : 0);
+
 		tp->snd_una = th->th_ack;
+
 		if (tp->t_flags & TF_SACK_PERMIT) {
 			if (SEQ_GT(tp->snd_una, tp->snd_recover))
 				tp->snd_recover = tp->snd_una;
@@ -2753,8 +3832,15 @@
 				 * we should release the tp also, and use a
 				 * compressed state.
 				 */
+				mp_debug(MPSESSION, 1, 0,
+				    "%s: FIN is acked, enter FINWAIT_2 tp %p\n", __func__, tp);
 				if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
-					soisdisconnected(so);
+					// call into mptcp layer function to do this
+					// are any subflows in state < TCPS_FIN_WAIT_2
+					// if so then skip past the soisdisconnected
+					// (in fact soisdiconnected called from the
+					// mp_layer function)
+
 					tcp_timer_activate(tp, TT_2MSL,
 					    (tcp_fast_finwait2_recycle ?
 					    tcp_finwait2_timeout :
@@ -2831,7 +3917,7 @@
 		 * actually wanting to send this much urgent data.
 		 */
 		SOCKBUF_LOCK(&so->so_rcv);
-		if (th->th_urp + so->so_rcv.sb_cc > sb_max) {
+		if (th->th_urp + (so->so_rcv.sb_cc) > sb_max) {
 			th->th_urp = 0;			/* XXX */
 			thflags &= ~TH_URG;		/* XXX */
 			SOCKBUF_UNLOCK(&so->so_rcv);	/* XXX */
@@ -2853,7 +3939,7 @@
 		 */
 		if (SEQ_GT(th->th_seq+th->th_urp, tp->rcv_up)) {
 			tp->rcv_up = th->th_seq + th->th_urp;
-			so->so_oobmark = so->so_rcv.sb_cc +
+			so->so_oobmark = (so->so_rcv.sb_cc) +
 			    (tp->rcv_up - tp->rcv_nxt) - 1;
 			if (so->so_oobmark == 0)
 				so->so_rcv.sb_state |= SBS_RCVATMARK;
@@ -2886,7 +3972,7 @@
 
 	/*
 	 * Process the segment text, merging it into the TCP sequencing queue,
-	 * and arranging for acknowledgment of receipt if necessary.
+	 * and arranging for acknowledgement of receipt if necessary.
 	 * This process logically involves adjusting tp->rcv_wnd as data
 	 * is presented to the user (this happens in tcp_usrreq.c,
 	 * case PRU_RCVD).  If a FIN has already been received on this
@@ -2896,7 +3982,13 @@
 	    TCPS_HAVERCVDFIN(tp->t_state) == 0) {
 		tcp_seq save_start = th->th_seq;
 		m_adj(m, drop_hdrlen);	/* delayed header drop */
+
+		if (thflags & TH_FIN)
+			mp_debug(MPSESSION, 1, 0, "%s:%d: got FIN on tp %p\n", __func__,
+			    __LINE__, tp);
+
 		/*
+		 * XXXNJW: Need to adapt this comment
 		 * Insert segment which includes th into TCP reassembly queue
 		 * with control block tp.  Set thflags to whether reassembly now
 		 * includes a segment with FIN.  This handles the common case
@@ -2908,35 +4000,23 @@
 		 * immediately when segments are out of order (so
 		 * fast retransmit can work).
 		 */
-		if (th->th_seq == tp->rcv_nxt &&
-		    LIST_EMPTY(&tp->t_segq) &&
-		    TCPS_HAVEESTABLISHED(tp->t_state)) {
-			if (DELAY_ACK(tp, tlen))
-				tp->t_flags |= TF_DELACK;
-			else
+		 thflags = tcp_reass(tp, th, &tlen, m);
+			if (TCPS_HAVEESTABLISHED(tp->t_state) &&
+					!tp->t_segq->disordered) {
+				if (DELAY_ACK(tp, tlen))
+					tp->t_flags |= TF_DELACK;
+				else
+					tp->t_flags |= TF_ACKNOW;
+				TCPSTAT_INC(tcps_rcvpack);
+				TCPSTAT_ADD(tcps_rcvbyte, tlen);
+			} else {
+				/* disordered bytes in list
+				 *
+				 * dont want to set FIN just yet
+				 */
 				tp->t_flags |= TF_ACKNOW;
-			tp->rcv_nxt += tlen;
-			thflags = th->th_flags & TH_FIN;
-			TCPSTAT_INC(tcps_rcvpack);
-			TCPSTAT_ADD(tcps_rcvbyte, tlen);
-			ND6_HINT(tp);
-			SOCKBUF_LOCK(&so->so_rcv);
-			if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
-				m_freem(m);
-			else
-				sbappendstream_locked(&so->so_rcv, m);
-			/* NB: sorwakeup_locked() does an implicit unlock. */
-			sorwakeup_locked(so);
-		} else {
-			/*
-			 * XXX: Due to the header drop above "th" is
-			 * theoretically invalid by now.  Fortunately
-			 * m_adj() doesn't actually frees any mbufs
-			 * when trimming from the head.
-			 */
-			thflags = tcp_reass(tp, th, &tlen, m);
-			tp->t_flags |= TF_ACKNOW;
-		}
+			}
+
 		if (tlen > 0 && (tp->t_flags & TF_SACK_PERMIT))
 			tcp_update_sack_list(tp, save_start, save_start + tlen);
 #if 0
@@ -2962,7 +4042,15 @@
 	 */
 	if (thflags & TH_FIN) {
 		if (TCPS_HAVERCVDFIN(tp->t_state) == 0) {
-			socantrcvmore(so);
+			mp_debug(MPSESSION, 1, 0, "%s:%d: got FIN on tp %p\n", __func__,
+			   __LINE__, tp);
+			/*
+			 * XXXNJW: Should only set SBS_CANTRCVMORE when the DFIN is rx'd
+			 * Will set all subflows tp can't receive more. TBD: on DFIN only
+			 */
+			if ((so->so_rcv.sb_state & SBS_CANTRCVMORE) == 0)
+				mp_set_cantrcvmore(tp);
+
 			/*
 			 * If connection is half-synchronized
 			 * (ie NEEDSYN flag on) then delay ACK,
@@ -3186,8 +4274,11 @@
 tcp_dooptions(struct tcpopt *to, u_char *cp, int cnt, int flags)
 {
 	int opt, optlen;
-
+	uint8_t subtype;
+
+	to->to_mopts.mpo_flags = 0;
 	to->to_flags = 0;
+	
 	for (; cnt > 0; cnt -= optlen, cp += optlen) {
 		opt = cp[0];
 		if (opt == TCPOPT_EOL)
@@ -3231,6 +4322,13 @@
 			    (char *)&to->to_tsecr, sizeof(to->to_tsecr));
 			to->to_tsecr = ntohl(to->to_tsecr);
 			break;
+		case TCPOPT_MPTCP:
+			bcopy((char *)cp + 2,
+					(char *)&subtype, sizeof(subtype));
+			if (optlen > MAX_MP_OPLEN)
+				continue;
+			mp_dosubtypes(to, subtype, cp, opt, optlen, flags);
+			break;
 #ifdef TCP_SIGNATURE
 		/*
 		 * XXX In order to reply to a host which has set the
@@ -3688,7 +4786,7 @@
  * be started again.
  */
 static void
-tcp_newreno_partial_ack(struct tcpcb *tp, struct tcphdr *th)
+tcp_newreno_partial_ack(struct tcpcb *tp, struct tcphdr *th, uint32_t acked)
 {
 	tcp_seq onxt = tp->snd_nxt;
 	u_long  ocwnd = tp->snd_cwnd;
@@ -3702,7 +4800,7 @@
 	 * Set snd_cwnd to one segment beyond acknowledged offset.
 	 * (tp->snd_una has not yet been updated when this function is called.)
 	 */
-	tp->snd_cwnd = tp->t_maxseg + BYTES_THIS_ACK(tp, th);
+	tp->snd_cwnd = tp->t_maxseg + acked;
 	tp->t_flags |= TF_ACKNOW;
 	(void) tcp_output(tp);
 	tp->snd_cwnd = ocwnd;
@@ -3712,8 +4810,8 @@
 	 * Partial window deflation.  Relies on fact that tp->snd_una
 	 * not updated yet.
 	 */
-	if (tp->snd_cwnd > BYTES_THIS_ACK(tp, th))
-		tp->snd_cwnd -= BYTES_THIS_ACK(tp, th);
+	if (tp->snd_cwnd > acked)
+		tp->snd_cwnd -= acked;
 	else
 		tp->snd_cwnd = 0;
 	tp->snd_cwnd += tp->t_maxseg;
diff -r 1e55e1aa8b7f sys/netinet/tcp_output.c
--- a/sys/netinet/tcp_output.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_output.c	Fri Jul 11 13:32:06 2014 +1000
@@ -37,10 +37,13 @@
 #include "opt_ipsec.h"
 #include "opt_tcpdebug.h"
 
+#include <sys/endian.h>
+
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/domain.h>
 #include <sys/hhook.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/lock.h>
 #include <sys/mbuf.h>
@@ -50,6 +53,7 @@
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 
 #include <net/if.h>
 #include <net/route.h>
@@ -80,7 +84,8 @@
 #ifdef TCP_OFFLOAD
 #include <netinet/tcp_offload.h>
 #endif
-
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
 #ifdef IPSEC
 #include <netipsec/ipsec.h>
 #endif /*IPSEC*/
@@ -94,7 +99,7 @@
 	&VNET_NAME(path_mtu_discovery), 1,
 	"Enable Path MTU Discovery");
 
-VNET_DEFINE(int, tcp_do_tso) = 1;
+VNET_DEFINE(int, tcp_do_tso) = 0;
 #define	V_tcp_do_tso		VNET(tcp_do_tso)
 SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, tso, CTLFLAG_RW,
 	&VNET_NAME(tcp_do_tso), 0,
@@ -128,6 +133,9 @@
 			    long len, int tso);
 static void inline	cc_after_idle(struct tcpcb *tp);
 
+static void inline 	mp_addoptions(struct tcpopt *to,
+				u_int *optlen_ptr, u_char *optp);
+
 /*
  * Wrapper for the TCP established output helper hook.
  */
@@ -161,6 +169,361 @@
 		CC_ALGO(tp)->after_idle(tp->ccv);
 }
 
+static void inline
+mp_addoptions(struct tcpopt *to, u_int *optlen_ptr, u_char *optp) {
+	struct mp_capable mcap;
+
+	uint64_t mask = 0;
+	for (mask = 1; mask < MPOF_MAXOPT; mask <<= 1) {
+		if ((to->to_mopts.mpo_flags & mask) != mask)
+			continue;
+		if (*optlen_ptr == TCP_MAXOLEN)
+			break;
+		switch (to->to_mopts.mpo_flags & mask)
+		{
+		case MPOF_CAPABLE_SYN:
+		{
+			/*
+			 * Note that keys are stored in network order, so
+			 * do no need to call htobe64 before bcopy
+			 */
+			mcap.kind = TCPOPT_MPTCP;
+			mcap.ver_sub = MPTCP_SUBTYPE_MP_CAPABLE;
+
+			uint8_t cap_flags = 0;
+			cap_flags |= USE_SHA1; // hard coded in sha1
+			cap_flags &= ~USE_CSUM; // don't support csumming yet
+
+			if (to->to_mopts.mpo_flags & MPOF_USE_CSUM) {
+				cap_flags |= USE_CSUM;
+			}
+			/* sending a SYN */
+
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_CAPABLE_SYN)
+				break;
+
+			mcap.length = MPTCP_SUBLEN_MP_CAPABLE_SYN;
+
+			/* SYN (or SYNACK), just include the local key */
+			u_int64_t key = to->to_mopts.local_key;
+
+			/* MP_CAPABLE type/flags */
+			*optlen_ptr += MPTCP_SUBLEN_MP_CAPABLE_SYN;
+			*optp++ = mcap.kind;
+			*optp++ = mcap.length;
+			*optp++ = 0; // version and subtype are 0
+			*optp++ = cap_flags;
+
+			/* copy in the key s*/
+			bcopy((u_char *)&key, optp, sizeof(key));
+			optp += sizeof(key);
+
+			/*
+			 * Indicate to caller that the capable option has been
+			 * copied into the outgoing TCP header.
+			 */
+			to->to_mopts.mpo_flags &= ~MPOF_CAPABLE_SYN;
+			break;
+		}
+		case MPOF_CAPABLE_ACK:
+		{
+			uint8_t cap_flags = 0;
+			/* ACK of 3-way handshake */
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_CAPABLE_ACK)
+				break;
+
+			u_int64_t local_key = to->to_mopts.local_key;
+			u_int64_t remote_key = to->to_mopts.remote_key;
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_CAPABLE_ACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_CAPABLE_ACK;
+			*optp++ = 0;	// just manually setting
+			*optp++ = cap_flags;
+
+			/* and copy in the local key */
+			bcopy((u_char *)&local_key, optp, sizeof(local_key));
+			optp += sizeof(local_key);
+
+			/* echo the remote key */
+			bcopy((u_char *)&remote_key, optp, sizeof(remote_key));
+			optp += sizeof(remote_key);
+
+			break;
+		}
+		case MPOF_MP_JOIN: /* Sending a MP_JOIN SYN Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_SYN)
+				break;
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+
+			/*
+			 * XXXNJW: just assuming for now that SYNs are sent as subflows
+			 * are added
+			 */
+			uint8_t addrID = to->to_mopts.addr_id;
+
+			subtype = (subtype << 4);
+
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = (MPTCP_SUBTYPE_MP_JOIN << 4);	// XXXNJW manually setting
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = addrID;
+
+			bcopy((u_char *)&to->to_mopts.rcv_token,
+					optp, sizeof(to->to_mopts.rcv_token));
+			optp += sizeof(to->to_mopts.rcv_token);
+
+			bcopy((u_char *)&to->to_mopts.snd_rnd,
+					optp, sizeof(to->to_mopts.snd_rnd));
+			optp += sizeof(to->to_mopts.snd_rnd);
+
+			break;
+		}
+		case MPOF_JOIN_SYN: /* Sending a MP_JOIN SYNACK Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_SYNACK)
+				break;
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+			subtype = (subtype << 4);
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_JOIN_SYNACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_SYNACK;
+			*optp++ = subtype;
+			*optp++ = (uint8_t) to->to_mopts.addr_id;
+
+			bcopy((u_char *)&to->to_mopts.truncated_MAC,
+			    optp, sizeof(to->to_mopts.truncated_MAC));
+			optp += sizeof(to->to_mopts.truncated_MAC);
+
+			bcopy((u_char *)&to->to_mopts.snd_rnd,
+			    optp, sizeof(to->to_mopts.snd_rnd));
+			optp += sizeof(to->to_mopts.snd_rnd);
+			break;
+		}
+		case MPOF_JOIN_SYNACK: /* Sending a MP_JOIN ACK Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_ACK) {
+				break;
+			}
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+			subtype = (subtype << 4);
+
+			*optlen_ptr += (uint8_t) MPTCP_SUBLEN_MP_JOIN_ACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_ACK;
+			*optp++ = subtype;
+			/* the last 12-bits after subtype are reserved (zeroed) */
+			*optp++ = 0;
+
+			/* Copy full HMAC-A into option */
+			bcopy(to->to_mopts.snd_mac, optp, 20);
+			optp += 20;
+			break;
+		}
+		case MPOF_DSS:
+		{
+			uint8_t subtype = MPTCP_SUBTYPE_DSS;
+			uint8_t dss_opts = 0;
+			uint8_t length = 4; /* the initial 4 bytes of option */
+
+			/* Set flags for DSS */
+	//		if (to->to_mopts.mpo_flags & MPOF_DATA_FIN) {
+	//			printf("adding Data-fin\n");
+	//			dss_opts |= FIN_PRESENT;
+	//		}
+
+			if (to->to_mopts.mpo_flags & MPOF_DATA_ACK) {
+				dss_opts |= ACK_PRESENT;
+				length += 4;
+
+				if (to->to_mopts.mpo_flags & MPOF_ACK64) {
+					dss_opts |= ACK_64_PRESENT;
+					length += 4;
+				}
+			}
+
+			if (to->to_mopts.mpo_flags & MPOF_DSN_MAP) {
+				dss_opts |= MAP_PRESENT;
+				length += 4;
+
+				length += 4;	/* subflow sequence num */
+				length += 2;	/* data length */
+
+				if(to->to_mopts.mpo_flags & MPOF_USE_CSUM)
+					length += 2;
+
+				if (to->to_mopts.mpo_flags & MPOF_DSN64) {
+					length += 4;
+					dss_opts |= DSN_64;
+				}
+			}
+
+			if (TCP_MAXOLEN - *optlen_ptr < length)
+				break;
+
+			*optlen_ptr += length;
+
+			// pad it out to divide by 4 to prevent EOL being inserted
+			// before our option
+			// XXXNJW kind of a hack, to fix
+			if (*optlen_ptr % 4) {
+				*optlen_ptr += TCPOLEN_NOP;
+				*optp++ = TCPOPT_NOP;
+				*optlen_ptr += TCPOLEN_NOP;
+				*optp++ = TCPOPT_NOP;
+			}
+
+			/* option kind and length fields */
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = length;
+
+			/* subtype and flags */
+			subtype = (subtype << 4);
+			bcopy((u_char *)&subtype, optp, sizeof(uint8_t));
+			optp += sizeof(uint8_t);
+			to->to_mopts.dss_opts_f_p = (uint8_t*) optp;
+			bcopy((u_char *)&dss_opts, optp, sizeof(uint8_t));
+			optp += sizeof(uint8_t);
+
+			/* copy the Data ACK */
+			if (to->to_mopts.mpo_flags & MPOF_DATA_ACK) {
+				if (to->to_mopts.mpo_flags & MPOF_ACK64) {
+					u_int64_t data_ack_num = to->to_mopts.data_ack_num;
+					data_ack_num = htobe64(data_ack_num);
+					bcopy((u_char *)&data_ack_num, optp, sizeof(data_ack_num));
+					optp += sizeof(data_ack_num);
+				} else {
+					u_int32_t data_ack_num =
+					    (uint32_t) to->to_mopts.data_ack_num;
+					data_ack_num = htonl(data_ack_num);
+					bcopy((u_char *)&data_ack_num, optp, sizeof(data_ack_num));
+					optp += sizeof(data_ack_num);
+				}
+			}
+
+			/* DSN contents */
+			if (to->to_mopts.mpo_flags & MPOF_DSN_MAP) {
+				uint32_t data_seq_num = (uint32_t) to->to_mopts.data_seq_num;
+				/* DSN and SSN */
+				if (to->to_mopts.mpo_flags & MPOF_DSN64) {
+					to->to_mopts.data_seq_num =
+							htobe64(to->to_mopts.data_seq_num);
+					bcopy((u_char *)&to->to_mopts.data_seq_num,
+							optp, sizeof(to->to_mopts.data_seq_num));
+					optp += sizeof(to->to_mopts.data_seq_num);
+				} else {
+					data_seq_num = htonl(data_seq_num);
+					bcopy((u_char *)&data_seq_num, optp, sizeof(data_seq_num));
+					optp += sizeof(data_seq_num);
+				}
+
+				to->to_mopts.sub_seq_num = htonl(to->to_mopts.sub_seq_num);
+				bcopy((u_char *)&to->to_mopts.sub_seq_num, optp,
+					sizeof(to->to_mopts.sub_seq_num));
+				optp += sizeof(to->to_mopts.sub_seq_num);
+
+				/* Length */
+				to->to_mopts.dss_data_len = htons(to->to_mopts.dss_data_len);
+				bcopy((u_char *)&to->to_mopts.dss_data_len, optp,
+					sizeof(to->to_mopts.dss_data_len));
+				optp += sizeof(to->to_mopts.dss_data_len);
+
+				if(to->to_mopts.mpo_flags & MPOF_USE_CSUM) {
+					to->to_mopts.dss_csum = htons(to->to_mopts.dss_csum);
+					bcopy((u_char *)&to->to_mopts.dss_csum, optp,
+						sizeof(to->to_mopts.dss_csum));
+					optp += sizeof(to->to_mopts.dss_csum);
+				}
+			}
+
+			/*
+			 * We process both at once so can clear flags to prevent
+			 * processing twice.
+			 */
+			to->to_mopts.mpo_flags &= ~MPOF_DSN_MAP;
+			to->to_mopts.mpo_flags &= ~MPOF_DATA_ACK;
+			break;
+		}
+		case MPOF_ADD_ADDR:
+		{
+			struct mp_add address;
+			int i, addr_id, addr_len = 0;
+			void * addr = NULL;
+
+			address.length = address.sub_ipver = 0;
+
+			/* The list of addresses should be locked at this point. */
+			/* Loop through the available addresses. */
+			for (i = 0; i < to->to_mopts.address_count; i++) {
+				/* continue if there are addresses to advertise */
+				if (!(to->to_mopts.add_addr_mask & (1 << i)))
+					continue;
+
+				switch (mp_usable_addresses[i].ss_family) {
+					case AF_INET:
+						addr = &((struct sockaddr_in *)
+							&mp_usable_addresses[i])->sin_addr.s_addr;
+						address.length = MPTCP_SUBLEN_ADD_ADDRV4;
+						address.sub_ipver = 4;
+						addr_len = 4;
+						break;
+					case AF_INET6:
+						addr = &((struct sockaddr_in6 *)
+							&mp_usable_addresses[i])->sin6_addr.__u6_addr;
+						address.length = MPTCP_SUBLEN_ADD_ADDRV6;
+						address.sub_ipver = 6;
+						addr_len = 16;
+						break;
+				}
+
+				if (TCP_MAXOLEN - *optlen_ptr < address.length)
+					break;
+
+				/* mark address as advertised. Need to pass this back to the
+				 * mpcb */
+				to->to_mopts.add_addr_mask &= ~(1 << i);
+
+				*optlen_ptr += address.length;
+				*optp++ = TCPOPT_MPTCP;
+				*optp++ = address.length;
+
+				/* subtype and IPVer */
+				uint8_t subtype = (MPTCP_SUBTYPE_ADD_ADDR << 4);
+				subtype |= address.sub_ipver;
+				bcopy((u_char *)&subtype, optp, sizeof(uint8_t));
+				optp ++;
+
+				/* Minus 1 as we never send add_addr for the master subflow */
+				addr_id = i - 1;
+
+				/* Address ID */
+				bcopy((u_char *)&addr_id, optp, sizeof(uint8_t));
+				optp ++;
+
+				bcopy((u_char *)addr, optp, addr_len);
+				optp += addr_len;
+			}
+			break;
+		}
+		case MPOF_REMOVE_ADDR:
+			break;
+		case MPOF_MP_FAIL:
+			break;
+		case MPOF_FASTCLOSE:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
 /*
  * Tcp output routine: figure out what should be sent and send it.
  */
@@ -169,7 +532,8 @@
 {
 	struct socket *so = tp->t_inpcb->inp_socket;
 	long len, recwin, sendwin;
-	int off, flags, error = 0;	/* Keep compiler happy */
+	int off = 0, flags, map_offset = 0, map_unsent, error = 0;	/* Keep compiler happy */
+	struct ds_map *snd_dsmap;
 	struct mbuf *m;
 	struct ip *ip = NULL;
 	struct ipovly *ipov = NULL;
@@ -217,6 +581,7 @@
 			idle = 0;
 		}
 	}
+
 again:
 	/*
 	 * If we've recently taken a timeout, snd_max will be greater than
@@ -229,10 +594,15 @@
 	sendalot = 0;
 	tso = 0;
 	mtu = 0;
+	flags = tcp_outflags[tp->t_state];
+
+	sendwin = min(tp->snd_wnd, tp->snd_cwnd);
 	off = tp->snd_nxt - tp->snd_una;
-	sendwin = min(tp->snd_wnd, tp->snd_cwnd);
+	KASSERT(SEQ_GEQ(tp->snd_nxt, tp->snd_una) && SEQ_LEQ(tp->snd_nxt, tp->snd_max),
+	    ("%s: snd_nxt not valid: %u, snd_una: %u, snd_max: %u",
+	    __func__, tp->snd_nxt, tp->snd_una, tp->snd_max));
 
-	flags = tcp_outflags[tp->t_state];
+
 	/*
 	 * Send any SACK-generated retransmissions.  If we're explicitly trying
 	 * to send out new data (when sendalot is 1), bypass this function.
@@ -240,6 +610,7 @@
 	 * we're replacing a (future) new transmission with a retransmission
 	 * now, and we previously incremented snd_cwnd in tcp_input().
 	 */
+
 	/*
 	 * Still in sack recovery , reset rxmit flag to zero.
 	 */
@@ -250,8 +621,8 @@
 	if ((tp->t_flags & TF_SACK_PERMIT) && IN_FASTRECOVERY(tp->t_flags) &&
 	    (p = tcp_sack_output(tp, &sack_bytes_rxmt))) {
 		long cwin;
-		
-		cwin = min(tp->snd_wnd, tp->snd_cwnd) - sack_bytes_rxmt;
+
+		cwin = sendwin - sack_bytes_rxmt;
 		if (cwin < 0)
 			cwin = 0;
 		/* Do not retransmit SACK segments beyond snd_recover */
@@ -269,6 +640,7 @@
 				 * moves past p->rxmit.
 				 */
 				p = NULL;
+				tp->snd_nxt = tp->snd_max;			//XXXNJW experimenting
 				goto after_sack_rexmit;
 			} else
 				/* Can rexmit part of the current hole */
@@ -276,15 +648,17 @@
 						   tp->snd_recover - p->rxmit));
 		} else
 			len = ((long)ulmin(cwin, p->end - p->rxmit));
-		off = p->rxmit - tp->snd_una;
-		KASSERT(off >= 0,("%s: sack block to the left of una : %d",
-		    __func__, off));
+		//off = p->rxmit - tp->snd_una;
+
 		if (len > 0) {
+			tp->snd_nxt = p->rxmit;					//XXXNJW experimenting
 			sack_rxmit = 1;
 			sendalot = 1;
 			TCPSTAT_INC(tcps_sack_rexmits);
 			TCPSTAT_ADD(tcps_sack_rexmit_bytes,
 			    min(len, tp->t_maxseg));
+		} else {
+			tp->snd_nxt = tp->snd_max;
 		}
 	}
 after_sack_rexmit:
@@ -297,7 +671,26 @@
 	if (tp->t_flags & TF_NEEDSYN)
 		flags |= TH_SYN;
 
-	SOCKBUF_LOCK(&so->so_snd);
+	/* XXXNJW IS_MASTER_TP test used to prevent maps from being added once
+	 * application has issued a close
+	 */
+	if((flags & TH_FIN && tp->snd_nxt == tp->snd_max)
+	    || tp->t_state >= TCPS_FIN_WAIT_1
+	    || (!IS_MASTER_TP(tp) && !tp->mp_active))
+		snd_dsmap = NULL;
+	else
+		snd_dsmap = mp_find_dsmap(tp, tp->snd_nxt);
+
+	if (snd_dsmap) {
+	    map_offset = off - (snd_dsmap->sf_seq_start - tp->snd_una);
+	    map_unsent = snd_dsmap->ds_map_len - map_offset;
+	} else {
+		map_unsent = 0;
+	}
+
+	// XXXNJW: double check the typcasting here.
+	recwin = (long) mp_get_recwin(tp->t_mpcb);
+
 	/*
 	 * If in persist timeout with window of 0, send 1 byte.
 	 * Otherwise, if window is small but nonzero
@@ -322,7 +715,8 @@
 			 * to send then the probe will be the FIN
 			 * itself.
 			 */
-			if (off < so->so_snd.sb_cc)
+			if ((map_offset < snd_dsmap->ds_map_len) && snd_dsmap ==
+				TAILQ_FIRST(&tp->t_txdsmaps))
 				flags &= ~TH_FIN;
 			sendwin = 1;
 		} else {
@@ -347,28 +741,31 @@
 	 * in which case len is already set.
 	 */
 	if (sack_rxmit == 0) {
-		if (sack_bytes_rxmt == 0)
-			len = ((long)ulmin(so->so_snd.sb_cc, sendwin) - off);
-		else {
+		if (sack_bytes_rxmt == 0) {
+			/* XXXNJW: sets len to 0 if the cwnd has dipped below a single map.
+			 * this is in place for debuggine only. in practice want to send
+			 * even partial maps */
+			int avail_window = (sendwin - off) > 1420 ? (sendwin - off) : 0;
+			len = (long)ulmin(map_unsent, avail_window);
+		} else {
 			long cwin;
 
-                        /*
+             /*
 			 * We are inside of a SACK recovery episode and are
 			 * sending new data, having retransmitted all the
 			 * data possible in the scoreboard.
 			 */
-			len = ((long)ulmin(so->so_snd.sb_cc, tp->snd_wnd) 
-			       - off);
+			len = (long)ulmin(map_unsent, tp->snd_wnd);
 			/*
 			 * Don't remove this (len > 0) check !
-			 * We explicitly check for len > 0 here (although it 
-			 * isn't really necessary), to work around a gcc 
+			 * We explicitly check for len > 0 here (although it
+			 * isn't really necessary), to work around a gcc
 			 * optimization issue - to force gcc to compute
 			 * len above. Without this check, the computation
 			 * of len is bungled by the optimizer.
 			 */
 			if (len > 0) {
-				cwin = tp->snd_cwnd - 
+				cwin = tp->snd_cwnd -
 					(tp->snd_nxt - tp->sack_newdata) -
 					sack_bytes_rxmt;
 				if (cwin < 0)
@@ -378,6 +775,9 @@
 		}
 	}
 
+//	if (sendwin - off < 0 && len > 0)
+//		kdb_break();
+
 	/*
 	 * Lop off SYN bit if it has already been sent.  However, if this
 	 * is SYN-SENT state and if segment contains data and if we don't
@@ -394,7 +794,8 @@
 	 * This measure is needed to prevent interoperability problems
 	 * with not fully conformant TCP implementations.
 	 */
-	if ((flags & TH_SYN) && (tp->t_flags & TF_NOOPT)) {
+	if (((flags & TH_SYN) && (tp->t_flags & TF_NOOPT))
+			|| (tp->t_mpcb->mp_flags & MPO_GOTJOINSYNACK)) {
 		len = 0;
 		flags &= ~TH_FIN;
 	}
@@ -423,6 +824,23 @@
 	/* len will be >= 0 after this point. */
 	KASSERT(len >= 0, ("[%s:%d]: len < 0", __func__, __LINE__));
 
+	if (len)
+		KASSERT(snd_dsmap != NULL, ("%s: Segment has length but no send map\n",
+		    __func__));
+
+//	if (snd_dsmap) {
+//		/* some debugging. this generally happens when sending at the upper
+//		 * end of the cwnd (there are some bytes but not enough to send a
+//		 * map/1-MSS. should wind up at 'just_return' */
+//		if (len < snd_dsmap->ds_map_len) {
+//			printf("%s: sendwin %ld len %ld map_len %u snd_nxt %u\n", __func__,
+//				sendwin, len, snd_dsmap->ds_map_len, tp->snd_nxt);
+//			printf("%s: map_unsent %d\n", __func__, map_unsent);
+//			if ((snd_dsmap->ds_map_len > 0) && (len == 0))
+//				kdb_break();
+//		}
+//	}
+
 	/*
 	 * Automatic sizing of send socket buffer.  Often the send buffer
 	 * size is not optimally adjusted to the actual network conditions
@@ -458,17 +876,25 @@
 	 * with congestion window.  Requires another timer.  Has to
 	 * wait for upcoming tcp timer rewrite.
 	 */
-	if (V_tcp_do_autosndbuf && so->so_snd.sb_flags & SB_AUTOSIZE) {
+
+	/* XXXNJW: Move snd buf adjustments into mp-level. Don't want individual
+	 * subflows adjusting the buffer sizes. should be the responsibility of
+	 * the scheduler. Having sf-level adjustments will probably limit the
+	 * throughput (eventually).
+	 */
+	SOCKBUF_LOCK(&so->so_snd);
+	if (V_tcp_do_autosndbuf && (so->so_snd.sb_flags & SB_AUTOSIZE)) {
 		if ((tp->snd_wnd / 4 * 5) >= so->so_snd.sb_hiwat &&
-		    so->so_snd.sb_cc >= (so->so_snd.sb_hiwat / 8 * 7) &&
-		    so->so_snd.sb_cc < V_tcp_autosndbuf_max &&
-		    sendwin >= (so->so_snd.sb_cc - (tp->snd_nxt - tp->snd_una))) {
+			so->so_snd.sb_cc >= (so->so_snd.sb_hiwat / 8 * 7) &&
+			so->so_snd.sb_cc < V_tcp_autosndbuf_max &&
+			sendwin >= (so->so_snd.sb_cc - (tp->snd_nxt - tp->snd_una))) {
 			if (!sbreserve_locked(&so->so_snd,
-			    min(so->so_snd.sb_hiwat + V_tcp_autosndbuf_inc,
-			     V_tcp_autosndbuf_max), so, curthread))
+				min(so->so_snd.sb_hiwat + V_tcp_autosndbuf_inc,
+				 V_tcp_autosndbuf_max), so, curthread))
 				so->so_snd.sb_flags &= ~SB_AUTOSIZE;
 		}
 	}
+	SOCKBUF_UNLOCK(&so->so_snd);
 
 	/*
 	 * Decide if we can use TCP Segmentation Offloading (if supported by
@@ -498,6 +924,9 @@
 	    tp->t_inpcb->in6p_options == NULL)
 		tso = 1;
 
+	/* XXXNJW: Must take care of this direct access to so_snd. Leave in for now
+	 * as this calculation is important when trying to rexmit an unanswered
+	 * FIN */
 	if (sack_rxmit) {
 		if (SEQ_LT(p->rxmit + len, tp->snd_una + so->so_snd.sb_cc))
 			flags &= ~TH_FIN;
@@ -506,13 +935,13 @@
 			flags &= ~TH_FIN;
 	}
 
-	recwin = sbspace(&so->so_rcv);
-
 	/*
 	 * Sender silly window avoidance.   We transmit under the following
 	 * conditions when len is non-zero:
 	 *
-	 *	- We have a full segment (or more with TSO)
+	 *	- We have a full segment (or more with TSO) OR
+	 *	  A map's size is smaller than the MSS, or we are at
+	 *	  the end of a map (bytes remaining are < MSS).
 	 *	- This is the last buffer in a write()/send() and we are
 	 *	  either idle or running NODELAY
 	 *	- we've timed out (e.g. persist timer)
@@ -521,18 +950,17 @@
 	 *	- we need to retransmit
 	 */
 	if (len) {
-		if (len >= tp->t_maxseg)
-			goto send;
+//		if (len >= tp->t_maxseg ||												//never the case with restricted, per-packet maps
+//		    (map_unsent > 0 && map_unsent == len))					      		// when we are running out of window len will be less
+		if (len >= 1420)
+			goto send;      													// than the remaining map to send (most likely)
 		/*
 		 * NOTE! on localhost connections an 'ack' from the remote
 		 * end may occur synchronously with the output and cause
 		 * us to flush a buffer queued with moretocome.  XXX
-		 *
-		 * note: the len + off check is almost certainly unnecessary.
 		 */
 		if (!(tp->t_flags & TF_MORETOCOME) &&	/* normal case */
 		    (idle || (tp->t_flags & TF_NODELAY)) &&
-		    len + off >= so->so_snd.sb_cc &&
 		    (tp->t_flags & TF_NOPUSH) == 0) {
 			goto send;
 		}
@@ -596,14 +1024,11 @@
 		/* 
 		 * If the new window size ends up being the same as the old
 		 * size when it is scaled, then don't force a window update.
+		 *
 		 */
 		if (oldwin >> tp->rcv_scale == (adv + oldwin) >> tp->rcv_scale)
 			goto dontupdate;
-
-		if (adv >= (long)(2 * tp->t_maxseg) &&
-		    (adv >= (long)(so->so_rcv.sb_hiwat / 4) ||
-		     recwin <= (long)(so->so_rcv.sb_hiwat / 8) ||
-		     so->so_rcv.sb_hiwat <= 8 * tp->t_maxseg))
+		if (adv >= (long) (2 * tp->t_maxseg))
 			goto send;
 	}
 dontupdate:
@@ -660,21 +1085,28 @@
 	 * if window is nonzero, transmit what we can,
 	 * otherwise force out a byte.
 	 */
-	if (so->so_snd.sb_cc && !tcp_timer_active(tp, TT_REXMT) &&
+	if (map_unsent && !tcp_timer_active(tp, TT_REXMT) &&
 	    !tcp_timer_active(tp, TT_PERSIST)) {
 		tp->t_rxtshift = 0;
 		tcp_setpersist(tp);
 	}
 
+//	/* sf was assigned a map, but wasn't able to transmit (e.g. due to limited
+//	 * send window). Make sure that SFF_DATA_WAIT is set so that the scheduler
+//	 * will use this sf next time through usr_send */
+//	if (tp->mp_active && snd_dsmap && ((tp->sf_flags & SFF_DATA_WAIT) == 0)) {
+//		tp->sf_flags |= SFF_DATA_WAIT;
+//		atomic_add_acq_int(&tp->t_mpcb->data_wait_count, 1);
+//		taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->data_task);
+//	}
+
 	/*
 	 * No reason to send a segment, just return.
 	 */
 just_return:
-	SOCKBUF_UNLOCK(&so->so_snd);
 	return (0);
 
 send:
-	SOCKBUF_LOCK_ASSERT(&so->so_snd);
 	/*
 	 * Before ESTABLISHED, force sending of initial options
 	 * unless TCP set not to do any options.
@@ -691,6 +1123,9 @@
 #endif
 		hdrlen = sizeof (struct tcpiphdr);
 
+	if (flags & TH_RST)
+		mp_debug(MPSESSION, 4, 0,"%s: state closed, in send\n", __func__);
+
 	/*
 	 * Compute options for segment.
 	 * We only have to care about SYN and established connection
@@ -699,6 +1134,8 @@
 	 */
 	if ((tp->t_flags & TF_NOOPT) == 0) {
 		to.to_flags = 0;
+		to.to_mopts.mpo_flags = 0;
+
 		/* Maximum segment size. */
 		if (flags & TH_SYN) {
 			tp->snd_nxt = tp->iss;
@@ -710,6 +1147,174 @@
 			to.to_wscale = tp->request_r_scale;
 			to.to_flags |= TOF_SCALE;
 		}
+
+		if (tp->mp_enabled) {
+			/*
+			 * If SYN and mp_enabled, should check if this is the initial
+			 * connection attempt on the master subflow. The master subflow
+			 * should have an MP_CAPABLE option, while a subflow will have
+			 * an MP_JOIN option
+			 */
+			if (flags & TH_SYN) {
+				/* Multipath announce capable, for SYN and SYN/ACK */
+				if (IS_MASTER_TP(tp)) {
+					tp->sent_capable = 1;
+
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_CAPABLE_SYN;
+
+					to.to_mopts.local_key =
+							tp->t_mpcb->local_key;
+					//if (tp->t_mpcb->mp_status & CSUM_ENABLED)
+						//to.to_mopts.mpo_flags |= MPOF_USE_CSUM;
+				} else {
+					/* A SYN on a subflow that isn't the master, use MP_JOIN
+					 * MP is active on this subflow, since we wouldn't try to
+					 * send a join on a non-mptcp connection. */
+					tp->sent_capable = 1;
+					tp->mpc = 1;
+					tp->mp_enabled = 1;
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_MP_JOIN;
+					to.to_mopts.rcv_token =
+							tp->t_mpcb->remote_token;
+					to.to_mopts.snd_rnd = arc4random();
+					// XXXNJW not good, but ensures add_id starts at "0"
+					to.to_mopts.addr_id = tp->t_mpcb->subflow_cnt - 1;
+					tp->local_rand = to.to_mopts.snd_rnd;
+				}
+			}
+
+			/* received a SYNACK with MP_CAPABLE */
+			if (tp->sf_flags & SFF_GOT_SYNACK) {
+				tp->sf_flags &= ~SFF_GOT_SYNACK;
+				/* got a synack after sending capable */
+
+				if (tp->sent_capable) {
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_CAPABLE_ACK;
+					to.to_mopts.local_key =
+							tp->t_mpcb->local_key;
+					to.to_mopts.remote_key =
+							tp->t_mpcb->remote_key;
+
+				}
+			}
+
+			/* Received a SYNACK with MP_JOIN */
+			if (tp->sf_flags & SFF_GOT_JOIN_SYNACK) {
+				/* XXX: should clear this only when we know that the
+				 * option has been copied into a packet.
+				 */
+				tp->sf_flags &= ~SFF_GOT_JOIN_SYNACK;
+
+				/* XXX: Should set this state only when we know the
+				 * option has been put into packet */
+				tp->sf_state = MPS_PRE_ESTABLISHED;
+
+				/* Must send full HMAC-A as third part of MP_JOIN
+				 * handshake */
+				to.to_mopts.snd_mac = tp->hmac_local;
+
+				/* Add an MP_JOIN_ACK to the outgoing packet */
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_JOIN_SYNACK;
+			}
+		}
+
+		/* do these options only once mptcp is established */
+		if ((tp->mp_active == 1) &&
+		    (tp->t_mpcb->mp_state >= MPS_ESTABLISHED)) {
+			
+			/*
+			 * Need to add an ADD_ADDR on the next outgoing packet if asked to
+			 * or if all available addresses have not yet been advertised
+			 */
+			if (tp->t_mpcb->mp_advaddr_mask) {
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_ADD_ADDR;
+				to.to_mopts.add_addr_mask = tp->t_mpcb->mp_advaddr_mask;
+				to.to_mopts.address_count = tp->t_mpcb->mp_conn_address_count;
+				to.to_mopts.addr_id = tp->t_mpcb->subflow_cnt - 1;
+
+				taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->pcb_create_task);
+			}
+
+			/*
+			 * If there are addresses that we have added that have not yet
+			 * attempted a join, schedule one now.
+			 */
+			if (tp->t_mpcb->mp_advjoin_mask)
+				taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->join_task);
+
+			/*
+			 * Need to attach a DATA-ACK to the outgoing packet
+			 * MPO_DATA_ACK is set in tcp_reass when in-order
+			 * data segments are provided to the process. currently
+			 * this works like an ACK_NOW, and will be put onto any
+			 * outgoing segment.
+			 *
+			 * XXX: provide a callout to do time-delayed Data-ACKs
+			 */
+
+			if ((tp->sf_flags & SFF_NEED_DACK) ||
+					(tp->t_mpcb->mp_flags & MPO_DATA_ACK)) {
+				tp->sf_flags &= ~SFF_NEED_DACK;
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= (MPOF_DATA_ACK | MPOF_DSS);
+				// should check against session settings
+				//to.to_mopts.mpo_flags |= MPOF_DSS;
+				//to.to_mopts.mpo_flags |= MPOF_ACK64;
+				to.to_mopts.data_ack_num =
+						(uint32_t) tp->t_mpcb->ds_rcv_nxt;
+			}
+
+			/* Only put a map on the first packet */
+			if (len && ((map_offset == 0 && snd_dsmap->ds_map_len > tp->t_maxseg)
+					|| (snd_dsmap->ds_map_len <= tp->t_maxseg)))
+				to.to_mopts.mpo_flags |= MPOF_DSN_MAP;
+
+			/* need to build a DSS MAP for the outgoing packet */
+			if (to.to_mopts.mpo_flags & MPOF_DSN_MAP) {
+				/* XXXNJW: implement checks for csum and dsn64 */
+				//to.to_mopts.mpo_flags |= MPOF_USE_CSUM; //XXX: temp
+				//to.to_mopts.mpo_flags |= MPOF_DSN64;	//XXX: temp
+
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_DSS;
+
+				/*
+				 * Add offset in the case of rexmits, inserting option in
+				 * multiple packets for a single map
+				 */
+				to.to_mopts.sub_seq_num =
+				    (snd_dsmap->sf_seq_start + map_offset - tp->iss);
+
+				//XXXNJW wrapping...
+				/*
+				 * Do not need to adjust ds mapping - we just re-send the
+				 * map start.
+				 */
+				to.to_mopts.data_seq_num =
+				    (uint32_t) snd_dsmap->ds_map_start + map_offset;			// added offset in case of partial rexmits
+
+				/* Length */
+				to.to_mopts.dss_data_len = snd_dsmap->ds_map_len - map_offset;	// adjust for the end of the mapping
+
+				/* received a SYNACK with MP_JOIN */
+				if (tp->t_mpcb->mp_flags & MPO_GOTJOINSYNACK) {
+					tp->t_mpcb->mp_flags &= ~MPO_GOTJOINSYNACK;
+					/* got a synack after sending capable */
+
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_JOIN_ACK;
+
+					// would also add the MAC/nonce here.
+				}
+
+			}
+		}
+
 		/* Timestamps. */
 		if ((tp->t_flags & TF_RCVD_TSTMP) ||
 		    ((flags & TH_SYN) && (tp->t_flags & TF_REQ_TSTMP))) {
@@ -743,6 +1348,12 @@
 		hdrlen += optlen = tcp_addoptions(&to, opt);
 	}
 
+	/* update the address masks */
+	if (to.to_mopts.mpo_flags & MPOF_ADD_ADDR) {
+		to.to_mopts.mpo_flags &= ~MPOF_ADD_ADDR;
+		tp->t_mpcb->mp_advaddr_mask = to.to_mopts.add_addr_mask;
+	}
+
 #ifdef INET6
 	if (isipv6)
 		ipoptlen = ip6_optlen(tp->t_inpcb);
@@ -757,6 +1368,7 @@
 	ipoptlen += ipsec_optlen;
 #endif
 
+
 	/*
 	 * Adjust data length if insertion of options will
 	 * bump the packet length beyond the t_maxopd length.
@@ -786,7 +1398,7 @@
 			 * fractional unless the send sockbuf can
 			 * be emptied.
 			 */
-			if (sendalot && off + len < so->so_snd.sb_cc) {
+			if (sendalot && map_offset + len < map_unsent) {
 				len -= len % (tp->t_maxopd - optlen);
 				sendalot = 1;
 			}
@@ -832,9 +1444,6 @@
 	 * the template for sends on this connection.
 	 */
 	if (len) {
-		struct mbuf *mb;
-		u_int moff;
-
 		if ((tp->t_flags & TF_FORCEDATA) && len == 1)
 			TCPSTAT_INC(tcps_sndprobe);
 		else if (SEQ_LT(tp->snd_nxt, tp->snd_max) || sack_rxmit) {
@@ -853,29 +1462,24 @@
 			m = m_gethdr(M_NOWAIT, MT_DATA);
 
 		if (m == NULL) {
-			SOCKBUF_UNLOCK(&so->so_snd);
 			error = ENOBUFS;
 			sack_rxmit = 0;
 			goto out;
 		}
-
 		m->m_data += max_linkhdr;
 		m->m_len = hdrlen;
 
 		/*
-		 * Start the m_copy functions from the closest mbuf
-		 * to the offset in the socket buffer chain.
+		 * Start the m_copy functions from the map's mbuf_start mbuf.
 		 */
-		mb = sbsndptr(&so->so_snd, off, len, &moff);
-
 		if (len <= MHLEN - hdrlen - max_linkhdr) {
-			m_copydata(mb, moff, (int)len,
-			    mtod(m, caddr_t) + hdrlen);
+			m_copydata(snd_dsmap->mbuf_start, snd_dsmap->mbuf_offset +
+			    map_offset, (int)len, mtod(m, caddr_t) + hdrlen);
 			m->m_len += len;
 		} else {
-			m->m_next = m_copy(mb, moff, (int)len);
+			m->m_next = m_copy(snd_dsmap->mbuf_start,
+				snd_dsmap->mbuf_offset + map_offset, (int)len);
 			if (m->m_next == NULL) {
-				SOCKBUF_UNLOCK(&so->so_snd);
 				(void) m_free(m);
 				error = ENOBUFS;
 				sack_rxmit = 0;
@@ -883,17 +1487,112 @@
 			}
 		}
 
+		if (len > sendwin)
+			kdb_break();
+
+		// XXXNJW need to address having a lock here.
+		MPCB_LOCK(tp->t_mpcb);
+
+		/*
+		 * XXXNJW: require an overhaul of tracking ds_snd_max. Currently the
+		 * async calls into
+		 */
+
+		/* Increment ds_snd_max by len if we are sending new data */
+		// should this not be the ds map start + len?
+		if (DSEQ_GT(tp->t_mpcb->ds_map_max, tp->t_mpcb->ds_snd_max))
+            tp->t_mpcb->ds_snd_max = tp->t_mpcb->ds_map_max;
+
+		if(tp->t_mpcb->doing_rxmit) {
+			mp_debug(DSMAP, 4, 0, "%s: d-rx, ds_snd_una: %u, tseq %u\n", __func__,
+			    (uint32_t) tp->t_mpcb->ds_snd_una, snd_dsmap->sf_seq_start);
+
+			/* just matching the original map for debugging purposes */
+			struct ds_map *a_map;
+			TAILQ_FOREACH(a_map, &tp->t_mpcb->mp_ds_sndmaps, mp_ds_map_next) {
+				if ((a_map->ds_map_start == snd_dsmap->ds_map_start)
+				    && (a_map != snd_dsmap)) { // don't match itself
+					mp_debug(DSMAP, 4, 0, "%s: %p org tseq %u, new tseq %u, "
+						"l %d\n", __func__, a_map, a_map->sf_seq_start,
+					    snd_dsmap->sf_seq_start, a_map->ds_map_len);
+				}
+			}
+
+		}
+
+//		if(map_unsent == 0)
+//			printf("%s: PSH map_start %u, len %u ds_snd_max %u\n", __func__,
+//			    (uint32_t) snd_dsmap->ds_map_start,
+//			    (uint32_t) snd_dsmap->ds_map_len,
+//			    (uint32_t) tp->t_mpcb->ds_snd_max);
+
+		MPCB_UNLOCK(tp->t_mpcb);
+
+		// error checking
+		if (tp->mp_active && tp->t_mpcb->ds_snd_una > tp->t_mpcb->ds_snd_max)
+			kdb_break();
+
+		/*
+		 * Complete sending data from the current map. Can allocate a new map
+		 * if there is more data available to send in the send buffer.
+		 */
+		if (map_offset + len >= snd_dsmap->ds_map_len) {
+			snd_dsmap->flags |= MAPF_IS_SENT;
+			mp_debug(DSMAP, 1, 0, "%s: Marked snd_dsmap %p as SENT, "
+				"map_len: %d, mbuf_start: %p, sf_seq_start: %u\n",
+				__func__, snd_dsmap, snd_dsmap->ds_map_len,
+				snd_dsmap->mbuf_start, snd_dsmap->sf_seq_start);
+
+			if(tp->t_mpcb->doing_rxmit)
+				mp_debug(DSMAP, 1, 0, "%s: Marked snd_dsmap %p as SENT, "
+					"dsn: %u, sf_seq_start: %u\n", __func__, snd_dsmap,
+					(uint32_t) snd_dsmap->ds_map_start,
+					snd_dsmap->sf_seq_start);
+
+			/*
+			 * XXXNJW: temporary
+			 * try to get a new map only if we dont need a FIN an
+			 * not in retransmit.
+			 */
+			if(((tp->t_flags & TF_NEEDFIN) &&
+			    snd_dsmap->sf_seq_start + snd_dsmap->ds_map_len == tp->snd_max)
+			    || (tp->t_rxtshift > 0))
+				snd_dsmap = NULL;
+			else {
+				/* XXXNJW: more data that can be sent? check for map now. this
+				 * will slow down multi-subflow connections (due to calling in
+				 * to find map and getting no return). */
+				snd_dsmap = mp_find_dsmap(tp, snd_dsmap->sf_seq_start +
+				   snd_dsmap->ds_map_len);
+			}
+
+			if (snd_dsmap != NULL) {
+				/* Have added more data that can be sent now */
+				map_unsent = snd_dsmap->ds_map_len;
+
+				/* If data level rexmit pending, override the scheduler and
+				 * send back-to-back (assuming room in sendwin). Also, if we
+				 * only have a single subflow, might as well send a windows
+				 * worth. */
+				if((snd_dsmap->flags & MAPF_IS_REXMIT) ||
+				    (tp->t_mpcb->subflow_cnt == 1))
+					sendalot = 1;
+			} else
+				map_unsent = 0;
+        }
+
 		/*
 		 * If we're sending everything we've got, set PUSH.
 		 * (This will keep happy those implementations which only
 		 * give data to the user when a buffer fills or
 		 * a PUSH comes in.)
-		 */
-		if (off + len == so->so_snd.sb_cc)
+		 *
+		 * Finished sending previous map and could not get more bytes
+		 * for this subflow */
+		if (map_unsent == 0) {
 			flags |= TH_PUSH;
-		SOCKBUF_UNLOCK(&so->so_snd);
+		}
 	} else {
-		SOCKBUF_UNLOCK(&so->so_snd);
 		if (tp->t_flags & TF_ACKNOW)
 			TCPSTAT_INC(tcps_sndacks);
 		else if (flags & (TH_SYN|TH_FIN|TH_RST))
@@ -918,7 +1617,6 @@
 		m->m_data += max_linkhdr;
 		m->m_len = hdrlen;
 	}
-	SOCKBUF_UNLOCK_ASSERT(&so->so_snd);
 	m->m_pkthdr.rcvif = (struct ifnet *)0;
 #ifdef MAC
 	mac_inpcb_create_mbuf(tp->t_inpcb, m);
@@ -945,6 +1643,7 @@
 	if (flags & TH_FIN && tp->t_flags & TF_SENTFIN &&
 	    tp->snd_nxt == tp->snd_max)
 		tp->snd_nxt--;
+
 	/*
 	 * If we are starting a connection, send ECN setup
 	 * SYN packet. If we are on a retransmit, we may
@@ -1022,11 +1721,12 @@
 	 * Calculate receive window.  Don't shrink window,
 	 * but avoid silly window syndrome.
 	 */
+	// XXXNJW should remove check against hiwater.
 	if (recwin < (long)(so->so_rcv.sb_hiwat / 4) &&
-	    recwin < (long)tp->t_maxseg)
+		recwin < (long)tp->t_maxseg)
 		recwin = 0;
 	if (SEQ_GT(tp->rcv_adv, tp->rcv_nxt) &&
-	    recwin < (long)(tp->rcv_adv - tp->rcv_nxt))
+		recwin < (long)(tp->rcv_adv - tp->rcv_nxt))
 		recwin = (long)(tp->rcv_adv - tp->rcv_nxt);
 	if (recwin > (long)TCP_MAXWIN << tp->rcv_scale)
 		recwin = (long)TCP_MAXWIN << tp->rcv_scale;
@@ -1080,6 +1780,7 @@
 	 * checksum extended header and data.
 	 */
 	m->m_pkthdr.len = hdrlen + len; /* in6_cksum() need this */
+	m->m_pkthdr.csum_flags = CSUM_TCP;
 	m->m_pkthdr.csum_data = offsetof(struct tcphdr, th_sum);
 #ifdef INET6
 	if (isipv6) {
@@ -1289,6 +1990,7 @@
 				tcp_timer_activate(tp, TT_PERSIST, 0);
 				tp->t_rxtshift = 0;
 			}
+
 			tcp_timer_activate(tp, TT_REXMT, tp->t_rxtcur);
 		}
 	} else {
@@ -1389,6 +2091,24 @@
 	tp->t_flags &= ~(TF_ACKNOW | TF_DELACK);
 	if (tcp_timer_active(tp, TT_DELACK))
 		tcp_timer_activate(tp, TT_DELACK, 0);
+
+//	if (sendalot) {
+//		sendwin -= len;
+//		if (sendwin < tp->t_maxseg)
+//			sendalot = 0;
+//	}
+
+	/* XXXNJW if we don't have room for an entire map, dont send.
+	 * This will be changed after testing, as we want to be able to send even
+	 * partial maps to make the most of whatever remains in the send window. */
+	if (sendalot) {
+		if ((sendwin - off - map_unsent) < tp->t_maxseg)
+			sendalot = 0;
+	}
+
+	if (sendalot && (sendwin < tp->t_maxseg))
+		kdb_break();
+
 #if 0
 	/*
 	 * This completely breaks TCP if newreno is turned on.  What happens
@@ -1401,6 +2121,7 @@
 #endif
 	if (sendalot)
 		goto again;
+
 	return (0);
 }
 
@@ -1523,6 +2244,15 @@
 				 *optp++ = 0;
 			break;
 			}
+		case TOF_MPTCP:
+			{
+			while (optlen % 2) {
+				optlen += TCPOLEN_NOP;
+				*optp++ = TCPOPT_NOP;
+			}
+			mp_addoptions(to, &optlen, optp);
+			break;
+			}
 		case TOF_SACK:
 			{
 			int sackblks = 0;
diff -r 1e55e1aa8b7f sys/netinet/tcp_reass.c
--- a/sys/netinet/tcp_reass.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_reass.c	Fri Jul 11 13:32:06 2014 +1000
@@ -37,6 +37,7 @@
 #include "opt_tcpdebug.h"
 
 #include <sys/param.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
@@ -69,12 +70,19 @@
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
+
 #include <netinet6/tcp6_var.h>
 #include <netinet/tcpip.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
 #endif /* TCPDEBUG */
 
+#define TAILQ_FOREACH_CONTINUE(var, head, field)                        \
+        for ((var) = ((var) ? (var) : TAILQ_FIRST((head)));             \
+        (var);                                                          \
+        (var) = TAILQ_NEXT((var), field))
+
 static SYSCTL_NODE(_net_inet_tcp, OID_AUTO, reass, CTLFLAG_RW, 0,
     "TCP Segment Reassembly Queue");
 
@@ -140,13 +148,14 @@
 
 	INP_WLOCK_ASSERT(tp->t_inpcb);
 
-	while ((qe = LIST_FIRST(&tp->t_segq)) != NULL) {
-		LIST_REMOVE(qe, tqe_q);
+	while ((qe = TAILQ_FIRST(tp->t_segq)) != NULL) {
+		TAILQ_REMOVE(tp->t_segq, qe, tqe_q);
 		m_freem(qe->tqe_m);
 		uma_zfree(V_tcp_reass_zone, qe);
 		tp->t_segqlen--;
 	}
 
+	tp->t_segqlen = 0;
 	KASSERT((tp->t_segqlen == 0),
 	    ("TCP reass queue %p segment count is %d instead of 0 after flush.",
 	    tp, tp->t_segqlen));
@@ -159,13 +168,19 @@
 	struct tseg_qent *p = NULL;
 	struct tseg_qent *nq;
 	struct tseg_qent *te = NULL;
+	struct tseg_qent *prev_disorder;
 	struct socket *so = tp->t_inpcb->inp_socket;
 	char *s = NULL;
 	int flags;
 	struct tseg_qent tqs;
+	struct mpcb *mp = tp->t_mpcb;
 
 	INP_WLOCK_ASSERT(tp->t_inpcb);
 
+	flags = 0;
+
+	SF_REASS_RLOCK(mp);
+
 	/*
 	 * XXX: tcp_reass() is rather inefficient with its data structures
 	 * and should be rewritten (see NetBSD for optimizations).
@@ -175,8 +190,12 @@
 	 * Call with th==NULL after become established to
 	 * force pre-ESTABLISHED data up to user socket.
 	 */
-	if (th == NULL)
-		goto present;
+	if (th == NULL) {
+		if(TAILQ_EMPTY(tp->t_segq))
+			goto unlock_return;
+		else
+			goto present;
+	}
 
 	/*
 	 * Limit the number of segments that can be queued to reduce the
@@ -194,19 +213,21 @@
 	 * Investigate why and re-evaluate the below limit after the behaviour
 	 * is understood.
 	 */
-	if ((th->th_seq != tp->rcv_nxt || !TCPS_HAVEESTABLISHED(tp->t_state)) &&
-	    tp->t_segqlen >= (so->so_rcv.sb_hiwat / tp->t_maxseg) + 1) {
-		V_tcp_reass_overflows++;
-		TCPSTAT_INC(tcps_rcvmemdrop);
-		m_freem(m);
-		*tlenp = 0;
-		if ((s = tcp_log_addrs(&tp->t_inpcb->inp_inc, th, NULL, NULL))) {
-			log(LOG_DEBUG, "%s; %s: queue limit reached, "
-			    "segment dropped\n", s, __func__);
-			free(s, M_TCPLOG);
-		}
-		return (0);
-	}
+
+	/* XXXNJW: Need to revisit this in re to receive window advertising */
+//	if (th->th_seq != tp->rcv_nxt &&
+//	    tp->t_segqlen >= (so->so_rcv.sb_hiwat / tp->t_maxseg) + 1) {
+//		V_tcp_reass_overflows++;
+//		TCPSTAT_INC(tcps_rcvmemdrop);
+//		m_freem(m);
+//		*tlenp = 0;
+//		if ((s = tcp_log_addrs(&tp->t_inpcb->inp_inc, th, NULL, NULL))) {
+//			log(LOG_DEBUG, "%s; %s: queue limit reached, "
+//			    "segment dropped\n", s, __func__);
+//			free(s, M_TCPLOG);
+//		}
+//		goto unlock_return;
+//	}
 
 	/*
 	 * Allocate a new queue entry. If we can't, or hit the zone limit
@@ -227,7 +248,7 @@
 				    "reached, segment dropped\n", s, __func__);
 				free(s, M_TCPLOG);
 			}
-			return (0);
+			goto unlock_return;
 		} else {
 			bzero(&tqs, sizeof(struct tseg_qent));
 			te = &tqs;
@@ -242,10 +263,28 @@
 	}
 	tp->t_segqlen++;
 
+	prev_disorder = tp->t_segq->disordered;
+
+	/*
+	 * 'fastpath' check
+	 */
+	if (th->th_seq == tp->rcv_nxt && !tp->t_segq->disordered) {
+		p = TAILQ_LAST(tp->t_segq, sf_reass_queue_head);
+		mp_debug(REASS, 5, 0, "%s: goto fastpath\n",__func__);
+		goto fastpath;
+	}
+
+	/*
+	 * setting to te is temp until we know exactly where
+	 * disorder starts
+	 */
+	if (tp->t_segq->disordered == NULL)
+		tp->t_segq->disordered = te;
+
 	/*
 	 * Find a segment which begins after this one does.
 	 */
-	LIST_FOREACH(q, &tp->t_segq, tqe_q) {
+	TAILQ_FOREACH(q, tp->t_segq, tqe_q) {
 		if (SEQ_GT(q->tqe_th->th_seq, th->th_seq))
 			break;
 		p = q;
@@ -269,12 +308,12 @@
 					uma_zfree(V_tcp_reass_zone, te);
 				tp->t_segqlen--;
 				/*
-				 * Try to present any queued data
-				 * at the left window edge to the user.
-				 * This is needed after the 3-WHS
-				 * completes.
+				 * Already have data in queue that this segment provided.
+				 * Queue is still disordered, return and send a DUP ACK
 				 */
-				goto present;	/* ??? */
+				mp_debug(REASS, 5, 0, "%s: queue still disordered, "
+				    "goto unlock_return\n",__func__);
+				goto unlock_return;
 			}
 			m_adj(m, i);
 			*tlenp -= i;
@@ -294,59 +333,389 @@
 		if (i <= 0)
 			break;
 		if (i < q->tqe_len) {
+			mp_debug(REASS, 5, 0, "%s: trim segment %u len %d, overlaps %u", __func__,
+			     th->th_seq,  *tlenp, q->tqe_th->th_seq);
 			q->tqe_th->th_seq += i;
+			q->tqe_dsn += i;
 			q->tqe_len -= i;
 			m_adj(q->tqe_m, i);
 			break;
 		}
-
-		nq = LIST_NEXT(q, tqe_q);
-		LIST_REMOVE(q, tqe_q);
+		nq = TAILQ_NEXT(q, tqe_q);
+		TAILQ_REMOVE(tp->t_segq, q, tqe_q);
 		m_freem(q->tqe_m);
 		uma_zfree(V_tcp_reass_zone, q);
 		tp->t_segqlen--;
 		q = nq;
 	}
 
+fastpath:
 	/* Insert the new segment queue entry into place. */
 	te->tqe_m = m;
 	te->tqe_th = th;
 	te->tqe_len = *tlenp;
+	te->tqe_dsn = 0;
+
+	if (te->tqe_len > 0) {
+		struct m_tag *mtag =  m_tag_locate(m, PACKET_COOKIE_MPTCP,
+			PACKET_TAG_DSN, NULL);
+		KASSERT(mtag != NULL, ("%s segment of len %d is missing an mbuf tag\n",
+			__func__, te->tqe_len));
+		te->tqe_dsn = ((struct dsn_tag *)mtag)->dsn;
+	}
 
 	if (p == NULL) {
-		LIST_INSERT_HEAD(&tp->t_segq, te, tqe_q);
+		mp_debug(REASS, 3, 0, "%s: insert segment at queue head, sqn: %u\n",
+		    __func__, th->th_seq);
+		/* Queue is empty so insert at head. */
+		TAILQ_INSERT_HEAD(tp->t_segq, te, tqe_q);
+		if (th->th_seq == tp->rcv_nxt) {
+			/*
+			 * Search from segment just inserted towards end of list looking
+			 * for a hole.
+			 */
+			q = te;
+			TAILQ_FOREACH_CONTINUE(q, tp->t_segq, tqe_q) {
+				nq = TAILQ_NEXT(q, tqe_q);
+				if (nq != NULL &&
+				    q->tqe_th->th_seq + q->tqe_len != nq->tqe_th->th_seq) {
+					/* Found the first out of sequence segment in
+					 * the reass list. */
+					tp->t_segq->disordered = nq;
+					break;
+				}
+			}
+			/* get through list without finding disorder */
+			if (q == NULL)
+				tp->t_segq->disordered = NULL;
+		} else {
+			tp->t_segq->disordered = TAILQ_FIRST(tp->t_segq);
+		}
 	} else {
+		/* Otherwise put it in sequence order in queue. */
+		mp_debug(REASS, 3, 0, "%s: insert segment in queue in sq order, "
+		    "sqn: %u, rcv_nxt: %u\n",__func__, th->th_seq, tp->rcv_nxt);
 		KASSERT(te != &tqs, ("%s: temporary stack based entry not "
 		    "first element in queue", __func__));
-		LIST_INSERT_AFTER(p, te, tqe_q);
+		TAILQ_INSERT_AFTER(tp->t_segq, p, te, tqe_q);
+
+		if (te == TAILQ_LAST(tp->t_segq, sf_reass_queue_head)) {
+			if(th->th_seq == tp->rcv_nxt) //inserted rcv_nxt at end of queue
+				tp->t_segq->disordered = NULL;
+		} else {
+			/* search through till we find a disorderd segment*/
+			q = TAILQ_FIRST(tp->t_segq);
+			TAILQ_FOREACH(q, tp->t_segq, tqe_q) {
+				nq = TAILQ_NEXT(q, tqe_q);
+				if (nq != NULL &&
+					q->tqe_th->th_seq + q->tqe_len != nq->tqe_th->th_seq) {
+					/* Found the first out of sequence segment
+					 * in the reass list. */
+					tp->t_segq->disordered = nq;
+					break;
+				}
+			}
+
+			/* Did not find any out of order segments, clear disordered. */
+			if (q == NULL)
+				tp->t_segq->disordered = NULL;
+		}
+
+	}
+	/* end insertion into segment queue */
+
+	if(tp->t_segq->disordered) {
+		if (prev_disorder == NULL)
+			mp_debug(REASS, 4, 0, "%s: queue became disordered at sqn %u after receiving sqn %u, rcv_nxt: %u, ds_rcv_nxt: %u\n",
+				__func__, tp->t_segq->disordered->tqe_th->th_seq, th->th_seq, tp->rcv_nxt, (uint32_t) mp->ds_rcv_nxt);
+		else if (prev_disorder != tp->t_segq->disordered)
+			mp_debug(REASS, 4, 0, "%s: queue remains disordered but progressed from sqn %u to sqn %u after receiving sqn %u, rcv_nxt: %u, ds_rcv_nxt: %u\n",
+		    __func__, prev_disorder->tqe_th->th_seq, tp->t_segq->disordered->tqe_th->th_seq, th->th_seq, tp->rcv_nxt, (uint32_t) mp->ds_rcv_nxt);
+		else
+			mp_debug(REASS, 4, 0, "%s: queue remains disordered at sqn %u after receiving sqn %u, rcv_nxt: %u, ds_rcv_nxt: %u\n",
+		    __func__, tp->t_segq->disordered->tqe_th->th_seq, th->th_seq, tp->rcv_nxt, (uint32_t) mp->ds_rcv_nxt);
+
+	} else {
+		if (prev_disorder)
+			mp_debug(REASS, 4, 0, "%s: queue transitioned from disordered at sqn %u to ordered after receiving sqn: %u\n",
+					__func__, prev_disorder->tqe_th->th_seq, th->th_seq);
+		else
+			mp_debug(REASS, 4, 0, "%s: queue is not disordered, sqn: %u, rcv_nxt: %u\n",__func__, th->th_seq, tp->rcv_nxt);
 	}
 
 present:
+
+	mp_debug(REASS, 5, 0, "%s: present sqn %u\n",__func__, th->th_seq);
+
 	/*
 	 * Present data to user, advancing rcv_nxt through
 	 * completed sequence space.
+	 *
+	 * If connection not established then return.
+	 *
+	 * If the queue is empty, or the first element is not the next expected,
+	 * then return.
 	 */
-	if (!TCPS_HAVEESTABLISHED(tp->t_state))
-		return (0);
-	q = LIST_FIRST(&tp->t_segq);
-	if (!q || q->tqe_th->th_seq != tp->rcv_nxt)
-		return (0);
+	if (!TCPS_HAVEESTABLISHED(tp->t_state)) {
+		goto unlock_return;
+	}
+
+	/*
+	 * If there is some amount of in-order data we know that rcv_nxt
+	 * can be set to the sequence number of the highest byte received.
+	 * If the previous element to the first disordered segment is NULL,
+	 * it means we have no in-order data.
+	 */
+	if (tp->t_segq->disordered) {
+		nq = TAILQ_PREV(tp->t_segq->disordered, sf_reass_queue_head, tqe_q);
+		if (nq != NULL) {
+			mp_debug(REASS, 3, 0, "%s: queue is disordered, but contains in "
+					"sequence data, changing rcv_nxt from: %u to %u\n",
+			    __func__, tp->rcv_nxt, nq->tqe_th->th_seq + nq->tqe_len);
+			tp->rcv_nxt = nq->tqe_th->th_seq + nq->tqe_len;
+		} else {
+			// in this else the first element of the list is disordered,
+			// return immediately
+			goto unlock_return;
+		}
+
+	} else {
+		nq = TAILQ_LAST(tp->t_segq, sf_reass_queue_head);
+		tp->rcv_nxt = nq->tqe_th->th_seq + nq->tqe_len;
+
+		/*
+		 * if the last segment in the list has a FIN and we are not disordered
+		 * indicate to tcp_input that a FIN has been delivered.
+		 */
+		flags = nq->tqe_th->th_flags & TH_FIN;
+
+		if (tp->mp_active) {
+			mp_debug(REASS, 3, 0, "%s: queue is ordered, "
+				"rcv_nxt changed from: %u to %u. diff: %u tail_dsn: %u\n",
+				__func__, tp->rcv_nxt, nq->tqe_th->th_seq + nq->tqe_len,
+				(nq->tqe_th->th_seq + nq->tqe_len - tp->rcv_nxt),
+				(uint32_t) nq->tqe_dsn);
+			mp_debug(REASS, 3, 0, "%s: queue is ordered, "
+				"first dsn: %u \n",
+				__func__, (uint32_t) TAILQ_FIRST(tp->t_segq)->tqe_dsn);
+
+		}
+	}
+
+	/* is this segment next in data sequence order? */
+	q = TAILQ_FIRST(tp->t_segq);
+
+	/* XXXNJW: need to detect wrapping */
+	KASSERT((q->tqe_dsn >= tp->t_mpcb->ds_rcv_nxt ||
+			q->tqe_dsn <= tp->t_mpcb->ds_rcv_nxt + tp->rcv_wnd)		// good test for in-window?
+			|| q->tqe_th->th_flags & TH_FIN,
+	    ("%s: Segment DSN %u less than ds_rcv_nxt %u\n", __func__,
+	    (uint32_t)q->tqe_dsn,
+	    (uint32_t)tp->t_mpcb->ds_rcv_nxt));
+
+	// XXXNJW: be careful about reading ds_rcv_nxt
+	if (tp->t_segq->disordered == q) {
+		if (tp->mp_enabled)
+			mp_debug(REASS, 4, 0, "%s: goto unlock return "
+					"sqn:%u, direct_map:%d, tqe_dsn: %u, "
+					"tqe_len: %d, ds_rcv_nxt %u, tp_rcv_nxt: %u\n",
+					__func__, th->th_seq, tp->infinite_map, (uint32_t)
+					q->tqe_dsn, q->tqe_len, (uint32_t)tp->t_mpcb->ds_rcv_nxt,
+					tp->rcv_nxt);
+		goto unlock_return;
+	}
+
+	SF_REASS_RUNLOCK(mp);
+
+	if(!tp->infinite_map) {
+		/* Schedule a data level ACK on next subflow through tcp_output */
+		//tp->sf_flags |= SFF_NEED_DACK;
+	}
+
+	/*
+	 * XXXNJW: This is a temporary hack to allow upcalls that
+	 * require a certain number of bytes (sb_cc) before calling
+	 * soreceive() to work with deferred reassembly. In these
+	 * cases reassembly occurs in the subflow context rather than
+	 * in the context of the receiving process.
+	 */
 	SOCKBUF_LOCK(&so->so_rcv);
-	do {
-		tp->rcv_nxt += q->tqe_len;
-		flags = q->tqe_th->th_flags & TH_FIN;
-		nq = LIST_NEXT(q, tqe_q);
-		LIST_REMOVE(q, tqe_q);
-		if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
-			m_freem(q->tqe_m);
-		else
-			sbappendstream_locked(&so->so_rcv, q->tqe_m);
-		if (q != &tqs)
-			uma_zfree(V_tcp_reass_zone, q);
-		tp->t_segqlen--;
-		q = nq;
-	} while (q && q->tqe_th->th_seq == tp->rcv_nxt);
+
+//	if (tp->mp_active) {
+//		if (tp->t_segq->disordered) {
+//			// XXXNJW checking for rcv_nxt passing
+//			if (SEQ_GT(tp->rcv_nxt, tp->t_segq->disordered->tqe_th->th_seq)) {
+//				printf("%s: rcv_nxt %u > 1st dis seg %u tp %p\n ", __func__,
+//					tp->rcv_nxt, tp->t_segq->disordered->tqe_th->th_seq, tp);
+//				kdb_break();
+//			}
+//		}
+//
+//		// checking for a significant difference between rcv_nxt and rcv_adv. this
+//		// would indicate a sequence space mess up. might trigger falsely on wraps
+//		// but should be okay for debug
+//		if((0xf0000000 & tp->rcv_adv) != (0xf0000000 & tp->rcv_nxt)) {
+//			printf("%s: rcv_adv: %u, rcv_next %u, tp %p\n", __func__, tp->rcv_adv,
+//				tp->rcv_nxt, tp);
+//			kdb_break();
+//		}
+//	}
+	//if (so->so_rcv.sb_upcall != NULL)
+		mp_do_reass(tp);
+
+	//XXXNJW: Does this belong here or in do_reass?
 	ND6_HINT(tp);
 	sorwakeup_locked(so);
+
+	return (flags);
+
+unlock_return:
+	SF_REASS_RUNLOCK(mp);
 	return (flags);
 }
+
+void
+mp_do_reass(struct tcpcb *tp) {
+
+//error = m_mbuftouio(uio, sb->sb_mb, len);
+
+	/* XXX this is where we walk the subflow lists looking for
+	 * a segment matching dss rcv_nxt (there will always be at
+	 * least one in here before sorwakeup is called).
+	 * as we find in-sequence segments, pass them up to wherever,
+	 * then take the segment out of the list and release the mbuf.
+	 *
+	 * could cheat and call appendstream here?
+	 */
+	struct socket *so = tp->t_inpcb->inp_socket;
+	struct mpcb *mp;
+	struct tseg_qent *nq = NULL;
+	struct tseg_qent *q = NULL;
+	struct sockbuf *sb;
+	uint64_t prev_ds_rcv_nxt;
+
+	sb = &so->so_rcv;
+
+	SOCKBUF_LOCK_ASSERT(sb);
+
+	if (!tp)
+		return;
+
+	mp = tp->t_mpcb;
+	KASSERT(mp != NULL,("%s: No mp control block for TCP flow\n", __func__));
+
+	mp->ds_last_dack_sent = prev_ds_rcv_nxt = mp->ds_rcv_nxt;
+
+	mp_debug(REASS, 3, 0, "%s:  "
+	    "ds_rcv_nxt %u, tp_rcv_nxt: %u\n",
+	    __func__, (uint32_t) tp->t_mpcb->ds_rcv_nxt, tp->rcv_nxt);
+
+	/*
+	 * Search through subflow lists for in-order segments. if
+	 * found, remove from the list, append to sb and release
+	 * the mbuf
+	 *
+	 * XXXNJW this is inefficient when there are lots of subflows
+	 * Need to change so that if an in-order segment is found, then
+	 */
+	SF_REASS_WLOCK(mp);
+
+	int seg_count = 0;
+
+	struct tcpcb *subflow_tp = NULL;
+start_reass_head_list:
+	SLIST_FOREACH(subflow_tp, &mp->tp_list, next_subflow) {
+		TAILQ_FOREACH(q, subflow_tp->t_segq, tqe_q) {
+			if (!q)
+				continue;
+
+			/* remove segments that have been passed by the sf rcv_nxt and
+			 * data_level rcv_nxt. Can happen due to rexmits */
+			if (q->tqe_dsn < mp->ds_rcv_nxt
+			    && q->tqe_th->th_seq < subflow_tp->rcv_nxt) {
+				mp_debug(REASS, 2, 0, "%s: removing bypassed segment %u\n",
+					__func__, (uint32_t) q->tqe_dsn);
+				TAILQ_REMOVE(subflow_tp->t_segq, q, tqe_q);
+				uma_zfree(V_tcp_reass_zone, q);
+				subflow_tp->t_segqlen--;
+				continue;
+			}
+
+			if (q->tqe_dsn == mp->ds_rcv_nxt) {
+				do {
+					/* Retransmits might cause data-level holes to be filled in
+					 * by alternate subflows. If the data-level has caught up
+					 * to whatever was received on the subflow, but the subflow
+					 * has not yet caught up its own rexmits (i.e. rcv_nxt is
+					 * still somewhere earlier than this segment), should skip
+					 * over the segment and wait for the data to be rexmitted.
+					 */
+					if (SEQ_GT(q->tqe_th->th_seq, subflow_tp->rcv_nxt)) {
+						mp_debug(REASS, 3, 0, "%s: skipped map %p dsn %u\n",
+						    __func__, q, (uint32_t) q->tqe_dsn);
+						q = TAILQ_NEXT(q, tqe_q);
+						continue;
+					}
+
+					/* If we are here and the first segment is disordered,
+					 * will need to stop and take a look as to why. */
+					// might want to catch when th_seq > rcv_nxt
+					// even when in data-seq order.
+					if (q == subflow_tp->t_segq->disordered) {
+						printf("%s: dis %p is ds_rcv_nxt\n", __func__, q);
+						kdb_break();
+					}
+
+					seg_count++;
+					mp->ds_rcv_nxt += q->tqe_len;
+					// just a test hard sync'ing sf seq nums's
+					if(tp->infinite_map)
+						tp->rcv_nxt = mp->ds_rcv_nxt;
+					nq = TAILQ_NEXT(q, tqe_q);
+					TAILQ_REMOVE(subflow_tp->t_segq, q, tqe_q);
+					if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
+						m_freem(q->tqe_m);
+					else
+						sbappendstream_locked(&so->so_rcv, q->tqe_m);
+					uma_zfree(V_tcp_reass_zone, q);
+					subflow_tp->t_segqlen--;
+					mp_debug(REASS, 5, 0, "*%s: appended dsn: %u\n", __func__,
+					    (uint32_t) q->tqe_dsn);
+					q = nq;
+					if (q)
+						mp_debug(REASS, 5, 0, "%s: next dsn: %u *\n", __func__,
+							(uint32_t) q->tqe_dsn);
+				} while (q && q->tqe_dsn == mp->ds_rcv_nxt && q != subflow_tp->t_segq->disordered);
+				goto start_reass_head_list;
+			} else if ((q->tqe_len == 0 &&
+				(q->tqe_th->th_flags & TH_FIN) && !tp->t_segq->disordered)
+				|| q->tqe_dsn < mp->ds_rcv_nxt) {
+					TAILQ_REMOVE(subflow_tp->t_segq, q, tqe_q);
+					m_freem(q->tqe_m);
+					uma_zfree(V_tcp_reass_zone, q);
+					subflow_tp->t_segqlen--;
+					/* XXXNJW: In the q->tqe_dsn < mp->ds_rcv_nxt case,
+					 * ideally want to continue from the next element in the
+					 * current list, rather than from the first element in the
+					 * first list. */
+					goto start_reass_head_list;
+			}
+		}
+	}
+
+	SF_REASS_WUNLOCK(mp);
+
+	mp_debug(REASS, 3, 0, "%s: exit vars"
+		"ds_rcv_nxt %u, tp_rcv_nxt: %u segments appended: %d\n",
+		__func__, (uint32_t) tp->t_mpcb->ds_rcv_nxt, tp->rcv_nxt, seg_count);
+
+	if(!tp->infinite_map && prev_ds_rcv_nxt < mp->ds_rcv_nxt) {
+		/* Schedule a data level ACK on next subflow through tcp_output */
+		mp->mp_flags |= MPO_DATA_ACK;
+
+		//OR schedule DACK for the current subflow?
+		//might this cause issues of DACKs being delayed?
+		tp->sf_flags |= SFF_NEED_DACK;
+	}
+}
+
diff -r 1e55e1aa8b7f sys/netinet/tcp_sack.c
--- a/sys/netinet/tcp_sack.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_sack.c	Fri Jul 11 13:32:06 2014 +1000
@@ -81,6 +81,7 @@
 #include <sys/systm.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
+#include <sys/limits.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
 #include <sys/proc.h>		/* for proc0 declaration */
@@ -115,6 +116,7 @@
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
 #include <netinet6/tcp6_var.h>
 #include <netinet/tcpip.h>
 #ifdef TCPDEBUG
@@ -570,7 +572,7 @@
  * the midst of sack recovery.
  */
 void
-tcp_sack_partialack(struct tcpcb *tp, struct tcphdr *th)
+tcp_sack_partialack(struct tcpcb *tp, struct tcphdr *th, uint32_t acked)
 {
 	int num_segs = 1;
 
@@ -578,7 +580,7 @@
 	tcp_timer_activate(tp, TT_REXMT, 0);
 	tp->t_rtttime = 0;
 	/* Send one or 2 segments based on how much new data was acked. */
-	if ((BYTES_THIS_ACK(tp, th) / tp->t_maxseg) >= 2)
+	if ((acked / tp->t_maxseg) >= 2)
 		num_segs = 2;
 	tp->snd_cwnd = (tp->sackhint.sack_bytes_rexmit +
 	    (tp->snd_nxt - tp->sack_newdata) + num_segs * tp->t_maxseg);
diff -r 1e55e1aa8b7f sys/netinet/tcp_subr.c
--- a/sys/netinet/tcp_subr.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_subr.c	Fri Jul 11 13:32:06 2014 +1000
@@ -42,9 +42,12 @@
 #include <sys/systm.h>
 #include <sys/callout.h>
 #include <sys/hhook.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/khelp.h>
+#include <sys/limits.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 #include <sys/jail.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
@@ -53,6 +56,7 @@
 #endif
 #include <sys/priv.h>
 #include <sys/proc.h>
+#include <sys/queue.h>
 #include <sys/sdt.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
@@ -92,6 +96,8 @@
 #include <netinet6/tcp6_var.h>
 #endif
 #include <netinet/tcpip.h>
+#include <netinet/mptcp_var.h>
+#include <crypto/sha1.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
 #endif
@@ -117,6 +123,10 @@
 
 #include <security/mac/mac_framework.h>
 
+/* for malloc'ing list heads */
+MALLOC_DECLARE(M_REASSLISTHEAD);
+MALLOC_DEFINE(M_REASSLISTHEAD, "reasshead", "Head of reass queue list");
+
 VNET_DEFINE(int, tcp_mssdflt) = TCP_MSS;
 #ifdef INET6
 VNET_DEFINE(int, tcp_v6mssdflt) = TCP6_MSS;
@@ -143,6 +153,7 @@
     &sysctl_net_inet_tcp_mss_check, "I",
     "Default TCP Maximum Segment Size");
 
+
 #ifdef INET6
 static int
 sysctl_net_inet_tcp_mss_v6_check(SYSCTL_HANDLER_ARGS)
@@ -166,6 +177,11 @@
    "Default TCP Maximum Segment Size for IPv6");
 #endif /* INET6 */
 
+VNET_DEFINE(unsigned int, tcp_override_isn) = 0;
+SYSCTL_VNET_UINT(_net_inet_tcp, OID_AUTO, override_isn, CTLFLAG_RW,
+    &VNET_NAME(tcp_override_isn), 0,
+    "Manually set the initial sequence number of TCP flows");
+
 /*
  * Minimum MSS we accept and use. This prevents DoS attacks where
  * we are forced to a ridiculous low MSS like 20 and send hundreds
@@ -231,6 +247,8 @@
 static char *	tcp_log_addr(struct in_conninfo *inc, struct tcphdr *th,
 		    void *ip4hdr, const void *ip6hdr);
 
+static void	tcp_sf_detach_locked(struct inpcb *inp);
+
 /*
  * Target size of TCP PCB hash tables. Must be a power of two.
  *
@@ -254,6 +272,13 @@
 	struct	osd		osd;
 };
 
+struct mppcb_mem {
+	struct	mpcb		mpcb;
+};
+
+static VNET_DEFINE(uma_zone_t, mpcb_zone);
+#define	V_mpcb_zone			VNET(mpcb_zone)
+
 static VNET_DEFINE(uma_zone_t, tcpcb_zone);
 #define	V_tcpcb_zone			VNET(tcpcb_zone)
 
@@ -372,6 +397,13 @@
 	uma_zone_set_max(V_tcpcb_zone, maxsockets);
 	uma_zone_set_warning(V_tcpcb_zone, "kern.ipc.maxsockets limit reached");
 
+	V_mpcb_zone = uma_zcreate("mpcb", sizeof(struct mppcb_mem),
+				    NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
+		uma_zone_set_max(V_mpcb_zone, maxsockets);
+
+	/* zone for segments arriving before the mapping */
+	tcp_unmapped_init();
+
 	tcp_tw_init();
 	syncache_init();
 	tcp_hc_init();
@@ -402,6 +434,8 @@
 	TUNABLE_INT_FETCH("net.inet.tcp.soreceive_stream", &tcp_soreceive_stream);
 	if (tcp_soreceive_stream) {
 #ifdef INET
+		tcp_usrreqs.pru_soreceive = soreceive_generic;
+#else
 		tcp_usrreqs.pru_soreceive = soreceive_stream;
 #endif
 #ifdef INET6
@@ -747,6 +781,7 @@
 {
 	struct tcpcb_mem *tm;
 	struct tcpcb *tp;
+
 #ifdef INET6
 	int isipv6 = (inp->inp_vflag & INP_IPV6) != 0;
 #endif /* INET6 */
@@ -805,6 +840,27 @@
 		tp->t_flags |= TF_SACK_PERMIT;
 	TAILQ_INIT(&tp->snd_holes);
 	tp->t_inpcb = inp;	/* XXX */
+
+	/* insert head of list of reass queue heads */
+	tp->t_segq = malloc(sizeof(struct sf_reass_queue_head),
+			M_REASSLISTHEAD, M_NOWAIT | M_ZERO);
+
+	/* list of segments that have been received but do not have a map.
+	 * We hold on to them in case the map is yet to arrive. */
+	tp->t_unmapped_segq = malloc(sizeof(struct sf_unmapped_queue_head),
+			M_REASSLISTHEAD, M_NOWAIT | M_ZERO);
+
+	/*
+	 * pointer to the reassembly list head, which is inside the
+	 * sf_reass_queues_list (i.e. the list of reassembly queues
+	 */
+	TAILQ_INIT(tp->t_segq);
+	TAILQ_INIT(tp->t_unmapped_segq);
+
+	/* dss map queue */
+	TAILQ_INIT(&tp->t_txdsmaps);
+	TAILQ_INIT(&tp->t_rxdsmaps);
+
 	/*
 	 * Init srtt to TCPTV_SRTTBASE (0), so we can tell that we have no
 	 * rtt estimate.  Set rttvar so that srtt + 4 * rttvar gives
@@ -827,6 +883,211 @@
 	return (tp);		/* XXX */
 }
 
+
+struct mpcb *
+mp_newmpcb(struct inpcb *inp)
+{
+	struct mppcb_mem *mpm;
+	struct mpcb *mp;
+
+	mpm = uma_zalloc(V_mpcb_zone, M_NOWAIT | M_ZERO);
+	if (mpm == NULL)
+		return (NULL);
+	mp = &mpm->mpcb;
+	mp->master_inpcb = inp;
+	mp->master_socket = inp->inp_socket;
+	mp->subflow_cnt = 1;
+	mp->laddr_mask = 0;
+	mp->raddr_mask = 0;
+	mp->in_def_drop = 0;	// unused for now (to remove)
+	mp->mp_state = MPS_CLOSED;
+	mp->mp_flags = 0;
+
+	/* Init list of subflow tpcbs */
+	SLIST_INIT(&mp->tp_list);
+
+	/* List of subflow segment lists */
+	STAILQ_INIT(&mp->sf_reass_queues_list);
+
+	/* List of subflow send lists */
+	TAILQ_INIT(&mp->mp_ds_sndmaps);
+
+	/* List of ds-level segments awaiting rexmit */
+	TAILQ_INIT(&mp->mp_rxtmitmaps);
+
+	/* Init task queue handlers
+	 * XXXNJW: Some of these tasks are temporary and should be removed at some
+	 * point (e.g. datascheduler_task_handler used to kick tcp_output of
+	 * subflows while a proper packet scheduler is missing */
+
+	/* Asynchronous sending of MP_JOINs */
+	TASK_INIT(&mp->join_task, 0, mp_join_task_handler, mp);
+	/* XXXNJW: General Aysnchronous tasks... */
+	TASK_INIT(&mp->data_task, 0, mp_datascheduler_task_handler, mp);
+	/* set up pcb's for addresses advertised */
+	TASK_INIT(&mp->pcb_create_task, 0, mp_sf_alloc_task_handler, mp);
+	/* Perform rexmits when d-level RTO fires */
+	TASK_INIT(&mp->rexmit_task, 0, mp_rexmit_task_handler, mp);
+
+	/* initialise mutex for control block */
+	MPCB_LOCK_INIT(mp);
+
+	/*
+	 * pcb created by a TCP listener? if so then this will be set to '1' in
+	 * tcp_syncache (just prior to setting tp state to SYN_RECEIVED
+	 * XXXNJW: This is used as a temporary method to prevent hosts from
+	 * creating multiple subflows over the same pair of advertised IP
+	 * addresses
+	 */
+	mp->passive_open = 0;
+
+	/* When the data-level retransmit timer fires, it will acquire the
+	 * mp*/
+	callout_init(&mp->mpt_rexmt, CALLOUT_MPSAFE);
+
+	/* Zero the added address count - this increments when we receive
+	 * add_addr from the remote host */
+	mp->mp_added_address_count = 0;
+
+	/* Number of addresses available to this mp at creation time. Value may
+	 * differ from the global address count due to e.g. an interface not being
+	 * available at this time. */
+	mp->mp_conn_address_count = mp_address_count;
+
+	/*
+	 * Set the masks. We clear bits as ADD_ADDRs and JOINS are sent
+	 * for each of the subflows.
+	 */
+	uint32_t bitmask = 0xffffffff;
+	mp->mp_advaddr_mask = ~(bitmask << mp->mp_conn_address_count);
+
+	/* Update the advertise address mask and count of addresses available to
+	 * the session */
+	mp_update_available_addresses(mp);
+
+	/*
+	 * init the REASS lock. used for:
+	 * sf_reass_queues_list: when user process does reassembly
+	 * t_segq: when segments are inserted into the reassembly list
+	 */
+	SF_REASS_LOCK_INIT(mp);
+
+	return (mp);		/* XXX */
+
+}
+
+/*
+ * Free everything. locks, lists etc etc
+ *
+ */
+void
+mp_discardcb(struct mpcb *mp)
+{
+	/* Release any ds_maps that remain */
+    struct ds_map *map, *map_next;
+    map = TAILQ_FIRST(&mp->mp_ds_sndmaps);
+    while (map != NULL) {
+            map_next = TAILQ_NEXT(map, mp_ds_map_next);
+            free(map, M_DSSMAP);
+            map = map_next;
+    }
+
+    /* Release the list of segment queue heads */
+    struct sf_reass_queue_head *q_h, *nq_h;
+    q_h = STAILQ_FIRST(&mp->sf_reass_queues_list);
+	while (q_h != NULL) {
+		nq_h = STAILQ_NEXT(q_h, tqe_next_head);
+		/* reass queue elements flushed in tcp_discardcb */
+		free(q_h, M_REASSLISTHEAD);
+		q_h = nq_h;
+	}
+
+	SF_REASS_LOCK_DESTROY(mp);
+	uma_zfree(V_mpcb_zone, mp);
+
+}
+
+
+/*
+ * Tally up the acked bytes and remove and mark as ACKED any maps that have
+ * been fully sent and also acknowledged.
+ * XXXNJW: To be inlined, renamed
+ */
+void
+tcp_ackmap(struct tcpcb *tp, uint32_t acked, uint64_t data_ack_num)
+{
+	struct ds_map *map;
+	int completed_a_map = 0, first_map = 1;
+
+	do {
+		map = TAILQ_LAST(&tp->t_txdsmaps, dsmapq_head);
+		if (map != NULL) {
+
+			/* This map could have been restransmitted on another subflow and
+			 * already acked at the data-level. In this case mbuf_start would
+			 * have been set to NULL, and it would have been removed from
+			 * mp_ds_sndmaps. Remove it from the map list and free the map. */
+//			if (map->flags & MAPF_IS_DUP) {
+//				printf("%s: map already acked at ds-level, deleting\n",
+//				    __func__);
+//				TAILQ_REMOVE(&tp->t_txdsmaps, map, sf_ds_map_next);
+//				free(map, M_DSSMAP);
+//				continue;
+//			}
+
+			if (first_map) {
+				if (map->sf_seq_start <= tp->snd_una)
+					acked += tp->snd_una -  map->sf_seq_start;
+				else /* una has wrapped */
+					acked += (UINT_MAX - map->sf_seq_start) + tp->snd_una;
+				first_map = 0;
+			}
+
+			/*
+			 * If acked more than the length of the current map, then we can
+			 * remove it from the transmit ds_maps list and classify this map
+			 * as having been 'ACKED'. The map itself will be deallocd at the
+			 * mp layer once the data-level has been acked.
+			 */
+			if (acked >= map->ds_map_len) {										// need to clear rexmit maps that were queued but not
+				KASSERT((map->flags & MAPF_IS_SENT) ||							// yet sent
+				    (map->flags & MAPF_IS_REXMIT),
+				    ("map %p acked but not marked as sent", map));
+
+				/* This ack cumulatively acks all bytes outstanding in this map. */
+				TAILQ_REMOVE(&tp->t_txdsmaps, map, sf_ds_map_next);
+				/* Must come after TAILQ_REMOVE so mp_deferred_sbdrop
+				 * can't trash the link. */
+				map->flags |= MAPF_IS_ACKED;
+
+				acked -= map->ds_map_len;
+				completed_a_map++;
+
+				if (tp->infinite_map && map->ds_map_start
+						== tp->t_mpcb->ds_snd_una) {
+					tp->t_mpcb->ds_snd_una += map->ds_map_len;
+				}
+			/* else: TAILQ is empty or no more maps in the list
+			 * are covered by this ack */
+			} else
+				map = NULL;
+
+		}
+	} while (map != NULL);
+
+	if (tp->infinite_map && completed_a_map) {
+		mp_deferred_sbdrop(tp->t_mpcb);
+	}
+
+	/*
+	 * Got a data-level ACK.
+	 * mp_data_ack() locks the mpcb
+	 */
+	if (data_ack_num > 0) {
+		mp_data_ack(tp->t_mpcb, data_ack_num);
+	}
+}
+
 /*
  * Switch the congestion control algorithm back to NewReno for any active
  * control blocks using an algorithm which is about to go away.
@@ -909,9 +1170,19 @@
 		TCPSTAT_INC(tcps_drops);
 	} else
 		TCPSTAT_INC(tcps_conndrops);
-	if (errno == ETIMEDOUT && tp->t_softerror)
-		errno = tp->t_softerror;
-	so->so_error = errno;
+
+	/* XXXNJW: will stop application from closing due to timeout. the subflow
+	 * will still get released and the connection can keep going. This is a
+	 * temp solution to prevent timeout out subflows from triggering a disco
+	 * pru call from the socket.
+	 */
+	if (tp->t_mpcb != NULL) {
+		if (tp->t_mpcb->subflow_cnt == 1) {
+			if (errno == ETIMEDOUT && tp->t_softerror)
+				errno = tp->t_softerror;
+			so->so_error = errno;
+		}
+	}
 	return (tcp_close(tp));
 }
 
@@ -1003,9 +1274,24 @@
 		tcp_hc_update(&inp->inp_inc, &metrics);
 	}
 
-	/* free the reassembly queue, if any */
+	/* free the reassembly queue, if any
+	 * XXXNJW: If a subflow has called into tcp_discard, could we still need
+	 * to retain the segments, in cases where there is some disorder and these
+	 * segments will be needed to reassemble later? */
 	tcp_reass_flush(tp);
 
+	/* free any unmapped segments being buffered */
+	tcp_unmapped_flush(tp);
+
+	/* free ds_maps from t_rxmaps */
+	struct ds_map *n1, *n2;
+	n1 = TAILQ_FIRST(&tp->t_rxdsmaps);
+    while (n1 != NULL) {
+            n2 = TAILQ_NEXT(n1, sf_ds_map_next);
+            free(n1, M_DSSMAP);
+            n1 = n2;
+    }
+
 #ifdef TCP_OFFLOAD
 	/* Disconnect offload device, if any. */
 	if (tp->t_flags & TF_TOE)
@@ -1020,6 +1306,52 @@
 
 	khelp_destroy_osd(tp->osd);
 
+	if (tp->t_mpcb != NULL) {
+		mp_debug(MPSESSION, 4, 0,"%s: subflow count: %d\n", __func__,
+		    tp->t_mpcb->subflow_cnt);
+
+		/* discard the mp if we are in the processing of closing the last */
+		if (tp->t_mpcb->subflow_cnt == 1 &&
+		    (so->so_state & SS_ISDISCONNECTED) == 0) {
+				/* Workaround that prevents an early socket free */
+				SOCK_LOCK(so);
+				inp->inp_socket->so_state |= SS_PROTOREF;
+				SOCK_UNLOCK(so);
+				inp->inp_flags |= INP_SOCKREF;
+
+				mp_debug(MPSESSION, 1, 0,"%s: destroy mp\n", __func__);
+				mp_discardcb(tp->t_mpcb);
+				mp_debug(MPSESSION, 1, 0,"%s: flush buffers\n", __func__);
+				soisdisconnected(inp->inp_socket);
+		} else {
+			/* Decrement the subflow count of the connection. subflow_cnt is
+			 * incremented once the pcb's are allocated and the subflow enters
+			 * LISTEN or SYN_SENT (sf_join_task_handler, sf_alloc_task_handler
+			 * XXNJW: Also just temporary */
+			mp_debug(MPSESSION, 4, 0,"%s: decrement sf count on tp %p\n",
+			    __func__, tp);
+			tp->t_mpcb->subflow_cnt -= 1;
+//				if (tp->t_mpcb->subflow_cnt != 1)
+			inp->inp_flags &= ~INP_SOCKREF;
+
+			/* XXXNJW: related to the temporary schedular. If the subflow was
+			 * waiting for data just decrement the counter, to keep the count
+			 * accurate (it may be inflated by one, causing the remaining
+			 * subflow to defer getting a map on every other call through
+			 * tcp_output. */
+			if (tp->sf_flags & SFF_DATA_WAIT)
+				atomic_subtract_acq_int(&tp->t_mpcb->data_wait_count, 1);
+
+			/* remove the subflow tp from the list of tp's on this mp
+			 * connection, so that it is not dereferenced in future calls to
+			 * tcp_usr_disconnect, etc. The inpcb is probably about to be
+			 * dropped and dealloced anyway so there's not much that can be
+			 * done with the tp after that. */
+			mp_drop_subflow_tp(tp->t_mpcb, tp);
+
+		}
+	}
+
 	CC_ALGO(tp) = NULL;
 	inp->inp_ppcb = NULL;
 	tp->t_inpcb = NULL;
@@ -1036,6 +1368,9 @@
 	struct inpcb *inp = tp->t_inpcb;
 	struct socket *so;
 
+	mp_debug(MPSESSION, 1, 0,"%s: entered, tp %p, mp_enabled %d\n", __func__,
+	    tp, tp->mp_enabled);
+
 	INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
 	INP_WLOCK_ASSERT(inp);
 
@@ -1047,21 +1382,119 @@
 	TCPSTAT_INC(tcps_closed);
 	KASSERT(inp->inp_socket != NULL, ("tcp_close: inp_socket NULL"));
 	so = inp->inp_socket;
-	soisdisconnected(so);
+
+	/*
+	 * For multipath connections, do not want to set the socket as
+	 * disconnected until the very last subflow is closing. However
+	 * subflows closing before that should still be detached.
+	 *
+	 * If this is a last remaining tp, then set stong socket reference flag
+	 * and set this inp as the pcb of the socket (the original inp might
+	 * have been released at this point).
+	 *
+	 * XXXNJW: temporary. to be redone when shutdown process is overhauled.
+	 */
+	if (tp->t_mpcb != NULL) {
+	    if (tp->t_mpcb->subflow_cnt == 1) {
+	    	if (tp->t_state >= TCPS_ESTABLISHED ||
+	    	    tp->t_state == TCPS_CLOSED) {
+				SOCK_LOCK(so);
+				so->so_state |= SS_PROTOREF;
+				SOCK_UNLOCK(so);
+				inp->inp_flags |= INP_SOCKREF;
+				so->so_pcb = (caddr_t)inp;
+	    	}
+	    	soisdisconnected(so);
+	    } else {
+	    	inp->inp_flags &= ~INP_SOCKREF;
+	    	mp_debug(MPSESSION, 1, 0, "%s: sf_detach on tp %p\n", __func__, tp);
+	    	tcp_sf_detach_locked(tp->t_inpcb);
+	    	return (NULL);
+	    }
+	} else {
+		soisdisconnected(so);
+	}
+
 	if (inp->inp_flags & INP_SOCKREF) {
 		KASSERT(so->so_state & SS_PROTOREF,
 		    ("tcp_close: !SS_PROTOREF"));
 		inp->inp_flags &= ~INP_SOCKREF;
 		INP_WUNLOCK(inp);
 		ACCEPT_LOCK();
+		KASSERT(inp != NULL, ("%s: inp == NULL", __func__));
 		SOCK_LOCK(so);
 		so->so_state &= ~SS_PROTOREF;
+		mp_debug(MPSESSION, 1, 0, "%s: calling sofree with tp %p\n", __func__,
+		    tp);
 		sofree(so);
 		return (NULL);
 	}
 	return (tp);
 }
 
+/*
+ * This is similar to the tcp_detach function in tcp_ussreq. However
+ * We are replacing call to inpcbdetach() with inp->inp_socket = NULL;
+ * This is done as inpcbdeteach will set so->so_pcb to NULL, which
+ * can cause KASSERT to fail when in multi-subflow connections. I.e.
+ * this function is only ever called by a subflow that isn't the last
+ * subflow being closed on the connection. The last subflow to close
+ * will detach the inp in the standard way.
+ */
+static void
+tcp_sf_detach_locked(struct inpcb *inp) {
+	struct tcpcb *tp;
+
+	tp = intotcpcb(inp);
+
+	if (inp->inp_flags & INP_TIMEWAIT) {
+		/*
+		 * There are two cases to handle: one in which the time wait
+		 * state is being discarded (INP_DROPPED), and one in which
+		 * this connection will remain in timewait.  In the former,
+		 * it is time to discard all state (except tcptw, which has
+		 * already been discarded by the timewait close code, which
+		 * should be further up the call stack somewhere).  In the
+		 * latter case, we detach from the socket, but leave the pcb
+		 * present until timewait ends.
+		 *
+		 * XXXRW: Would it be cleaner to free the tcptw here?
+		 */
+		if (inp->inp_flags & INP_DROPPED) {
+			KASSERT(tp == NULL, ("tcp_sf_detach_locked: INP_TIMEWAIT && "
+				"INP_DROPPED && tp != NULL"));
+			inp->inp_socket = NULL;
+			in_pcbfree(inp);
+		} else {
+			inp->inp_socket = NULL;
+			INP_WUNLOCK(inp);
+		}
+	} else {
+		/*
+		 * If the connection is not in timewait, we consider two
+		 * two conditions: one in which no further processing is
+		 * necessary (dropped || embryonic), and one in which TCP is
+		 * not yet done, but no longer requires the socket, so the
+		 * pcb will persist for the time being.
+		 *
+		 * XXXRW: Does the second case still occur?
+		 */
+		if (inp->inp_flags & INP_DROPPED ||
+			tp->t_state < TCPS_SYN_SENT) {
+			mp_debug(MPSESSION, 4, 0,
+			    "%s: discarding non-timewait/inp_dropped tp %p\n", __func__,
+			    tp);
+			tcp_discardcb(tp);
+			inp->inp_socket = NULL;
+			in_pcbfree(inp);
+		} else {
+			inp->inp_socket = NULL;
+			INP_WUNLOCK(inp);
+		}
+	}
+
+}
+
 void
 tcp_drain(void)
 {
@@ -1664,6 +2097,9 @@
 
 	INP_WLOCK_ASSERT(tp->t_inpcb);
 
+	if (V_tcp_override_isn)
+		return V_tcp_override_isn;
+
 	ISN_LOCK();
 	/* Seed if this is the first use, reseed if requested. */
 	if ((V_isn_last_reseed == 0) || ((V_tcp_isn_reseed_interval > 0) &&
@@ -2413,6 +2849,9 @@
 	int pstate = tp->t_state;
 #endif
 
+	mp_debug(MPSESSION, 1, 0, "%s: changing tp %p state to: %d\n", __func__,
+	    tp, newstate);
+
 	tp->t_state = newstate;
 	TCP_PROBE6(state__change, NULL, tp, NULL, tp, NULL, pstate);
 }
diff -r 1e55e1aa8b7f sys/netinet/tcp_syncache.c
--- a/sys/netinet/tcp_syncache.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_syncache.c	Fri Jul 11 13:32:06 2014 +1000
@@ -41,6 +41,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
+#include <sys/endian.h>
 #include <sys/sysctl.h>
 #include <sys/limits.h>
 #include <sys/lock.h>
@@ -53,6 +54,7 @@
 #include <sys/socketvar.h>
 #include <sys/syslog.h>
 #include <sys/ucred.h>
+#include <sys/queue.h>
 
 #include <sys/md5.h>
 #include <crypto/siphash/siphash.h>
@@ -78,11 +80,15 @@
 #include <netinet6/ip6_var.h>
 #include <netinet6/in6_pcb.h>
 #endif
+
+
 #include <netinet/tcp.h>
 #include <netinet/tcp_fsm.h>
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
 #include <netinet/tcp_syncache.h>
 #ifdef INET6
 #include <netinet6/tcp6_var.h>
@@ -143,6 +149,12 @@
 		    struct syncache *sc, struct tcphdr *th, struct tcpopt *to,
 		    struct socket *lso);
 #endif
+static struct	 socket *syncache_socket_subflow(struct syncache *, struct socket *,
+		    struct mbuf *m, struct sockaddr_in *nam);
+static inline struct inpcb*
+		inp_lookup(struct in_addr *faddr, struct in_addr *laddr, uint16_t fport,
+			uint16_t lport);
+
 
 /*
  * Transmit the SYN,ACK fewer times than TCP_MAXRXTSHIFT specifies.
@@ -659,7 +671,9 @@
 {
 	struct inpcb *inp = NULL;
 	struct socket *so;
+	struct mpcb *mp;
 	struct tcpcb *tp;
+
 	int error;
 	char *s;
 
@@ -835,9 +849,56 @@
 #endif /* INET */
 	INP_HASH_WUNLOCK(&V_tcbinfo);
 	tp = intotcpcb(inp);
+
+	/* create the multipath control block */
+	mp = mp_newmpcb(inp);
+	mp->master_tcpcb = tp;
+
+	/* put the master flow first in the list of subflows */
+	SLIST_INSERT_HEAD(&mp->tp_list, mp->master_tcpcb, next_subflow);
+
+	mp->subflow_cnt = 1;
+	tp->t_mpcb = mp;
+
+	/* XXXNJW: a workaround until path manager is implemented. Prevents a
+	 * host that is a passive opener from issuing MP_JOINs to addresses
+	 * added via ADD_ADDR if the passive host has more than one interface (i.e.
+	 * it will be send ADD_ADDRs of it's own). This means that the extra
+	 * mp_addresses are used in PASSIVE OPEN mode only. Prevents opening
+	 * multiple subflows between the same two pairs of addresses (and some
+	 * races with the async task handlers). */
+	mp->passive_open = 1;
+	mp_debug(MPSESSION, 5, 0, "%s: passive open: %d,\n", __func__,
+	     mp->passive_open);
+
+	/* should have a sysctl to check if we want to enable */
+
 	tcp_state_change(tp, TCPS_SYN_RECEIVED);
+
 	tp->iss = sc->sc_iss;
 	tp->irs = sc->sc_irs;
+
+	if (sc->is_master_subflow) {
+		tp->infinite_map = 1;
+		tp->t_mpcb->ds_map_max = tp->t_mpcb->ds_map_min = tp->t_mpcb->ds_snd_una = sc->sc_iss + 1;
+		tp->t_mpcb->ds_rcv_nxt = sc->sc_irs + 1;
+	}
+
+	/*
+	 * Only need to check for sent_capable as on the passive side if
+	 * this is set then mpc is also set.
+	 */
+	if (sc->sent_capable) {
+		tp->t_mpcb->csum_enabled = sc->csum_enabled;
+		tp->mpc = sc->mpc;
+		tp->sent_capable = 1;
+
+		if (sc->is_master_subflow) {
+			mp_process_local_key(tp->t_mpcb, sc->sc_local_key);
+			mp_process_remote_key(tp->t_mpcb, sc->sc_remote_key);
+		}
+	}
+
 	tcp_rcvseqinit(tp);
 	tcp_sendseqinit(tp);
 	tp->snd_wl1 = sc->sc_irs;
@@ -848,6 +909,8 @@
 	tp->rcv_adv += tp->rcv_wnd;
 	tp->last_ack_sent = tp->rcv_nxt;
 
+	tp->t_mpcb->ds_rcv_wnd = (uint32_t) tp->rcv_wnd;
+
 	tp->t_flags = sototcpcb(lso)->t_flags & (TF_NOPUSH|TF_NODELAY);
 	if (sc->sc_flags & SCF_NOOPT)
 		tp->t_flags |= TF_NOOPT;
@@ -924,6 +987,149 @@
 	return (NULL);
 }
 
+
+/*
+ * return the master socket for this subflow
+ */
+static struct socket *
+syncache_socket_subflow(struct syncache *sc, struct socket *lso,
+    struct mbuf *m, struct sockaddr_in *nam)
+{
+	struct inpcb *inp;
+	struct mpcb *mp;
+	struct socket *so;
+	struct tcpcb *tp;
+//	int error = 0;
+	int token;
+
+	token = sc->sc_mp_token;
+
+	/* Find the mp for the subflow connection token provided */
+	INP_INFO_LOCK_ASSERT(&V_tcbinfo);
+	LIST_FOREACH(inp, &V_tcb, inp_list) {
+		INP_WLOCK(inp);
+		/* Important to skip tcptw structs. */
+		if (!(inp->inp_flags & INP_TIMEWAIT) &&
+			(tp = intotcpcb(inp)) != NULL) {
+			if (tp->mp_active) {
+				if (inp == tp->t_mpcb->master_inpcb
+					&& tp->t_mpcb->local_token == token) {
+					break;
+				}
+			}
+		}
+		INP_WUNLOCK(inp);
+	}
+	KASSERT(inp != NULL,("%s: inp is null\n", __func__));
+
+	/* inp is the master inp at this point */
+	mp = intotcpcb(inp)->t_mpcb;
+	so = inp->inp_socket;
+
+	/* unlocking the master inp */
+	INP_WUNLOCK(inp);
+
+	/* Find the inp for this subflow */
+	inp = in_pcblookup_mbuf(&V_tcbinfo, nam->sin_addr,
+		nam->sin_port, sc->sc_inc.inc_laddr, sc->sc_inc.inc_lport,
+		INPLOOKUP_WILDCARD | INPLOOKUP_WLOCKPCB,
+		m->m_pkthdr.rcvif, m);
+
+	KASSERT(inp != NULL,("%s: inp is null (not found in lookup\n", __func__));						// XXXNJW to remove
+
+	inp->inp_inc.inc_flags = sc->sc_inc.inc_flags;
+	inp->inp_laddr = sc->sc_inc.inc_laddr;
+	inp->inp_lport = sc->sc_inc.inc_lport;
+
+	inp->inp_faddr = nam->sin_addr; //sc->sc_inc.inc_faddr;
+	inp->inp_fport = nam->sin_port;
+
+//	INP_HASH_WLOCK(&V_tcbinfo);
+//	in_pcbinshash(inp);
+//	INP_HASH_WUNLOCK(&V_tcbinfo);
+
+	tp = intotcpcb(inp);
+
+	tp->iss = sc->sc_iss;
+	tp->irs = sc->sc_irs;
+
+	tcp_rcvseqinit(tp);
+	tcp_sendseqinit(tp);
+	tp->snd_wl1 = sc->sc_irs;
+	tp->snd_max = tp->iss + 1;
+	tp->snd_nxt = tp->iss + 1;
+	tp->rcv_up = sc->sc_irs + 1;
+	tp->rcv_wnd = sc->sc_wnd;
+	tp->rcv_adv += tp->rcv_wnd;
+	tp->last_ack_sent = tp->rcv_nxt;
+
+	/* the HMACs generated during MP_JOIN handshake */
+	bcopy(sc->sc_hmac_local, tp->hmac_local, 20);
+	bcopy(sc->sc_hmac_remote, tp->hmac_remote, 20);
+
+	tp->t_flags = sototcpcb(lso)->t_flags & (TF_NOPUSH|TF_NODELAY);
+	if (sc->sc_flags & SCF_NOOPT)
+		tp->t_flags |= TF_NOOPT;
+	else {
+		if (sc->sc_flags & SCF_WINSCALE) {
+			tp->t_flags |= TF_REQ_SCALE|TF_RCVD_SCALE;
+			tp->snd_scale = sc->sc_requested_s_scale;
+			tp->request_r_scale = sc->sc_requested_r_scale;
+		}
+		if (sc->sc_flags & SCF_TIMESTAMP) {
+			tp->t_flags |= TF_REQ_TSTMP|TF_RCVD_TSTMP;
+			tp->ts_recent = sc->sc_tsreflect;
+			tp->ts_recent_age = tcp_ts_getticks();
+			tp->ts_offset = sc->sc_tsoff;
+		}
+#ifdef TCP_SIGNATURE
+		if (sc->sc_flags & SCF_SIGNATURE)
+			tp->t_flags |= TF_SIGNATURE;
+#endif
+		if (sc->sc_flags & SCF_SACK)
+			tp->t_flags |= TF_SACK_PERMIT;
+	}
+
+	if (sc->sc_flags & SCF_ECN)
+		tp->t_flags |= TF_ECN_PERMIT;
+
+	tp->mp_active = 1;
+	tp->mpc = 1;
+	tp->t_state = TCPS_SYN_RECEIVED;
+
+	/*
+	 * Set up MSS and get cached values from tcp_hostcache.
+	 * This might overwrite some of the defaults we just set.
+	 */
+	tcp_mss(tp, sc->sc_peer_mss);
+
+	/*
+	 * If the SYN,ACK was retransmitted, reset cwnd to 1 segment.
+	 * NB: sc_rxmits counts all SYN,ACK transmits, not just retransmits.
+	 */
+	if (sc->sc_rxmits > 1)
+		tp->snd_cwnd = tp->t_maxseg;
+
+	/*
+	 * Copy and activate timers.
+	 */
+	tp->t_keepinit = sototcpcb(lso)->t_keepinit;
+	tp->t_keepidle = sototcpcb(lso)->t_keepidle;
+	tp->t_keepintvl = sototcpcb(lso)->t_keepintvl;
+	tp->t_keepcnt = sototcpcb(lso)->t_keepcnt;
+	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
+
+	INP_WUNLOCK(inp);
+
+	TCPSTAT_INC(tcps_accepts);
+
+	return (so);
+
+//	SOCK_UNLOCK(so);
+//	INP_WUNLOCK(inp);
+//	return (NULL);
+}
+
 /*
  * This function gets called when we receive an ACK for a
  * socket in the LISTEN state.  We look up the connection
@@ -1070,7 +1276,154 @@
 	else
 		TCPSTAT_INC(tcps_sc_completed);
 
-/* how do we find the inp for the new socket? */
+	if (sc != &scs)
+		syncache_free(sc);
+	return (1);
+failed:
+	if (sc != NULL && sc != &scs)
+		syncache_free(sc);
+	if (s != NULL)
+		free(s, M_TCPLOG);
+	*lsop = NULL;
+	return (0);
+}
+
+/*
+ * XXXNJW: TBD
+ */
+int
+syncache_expand_subflow(struct in_conninfo *inc, struct tcpopt *to, struct tcphdr *th,
+    struct socket **lsop, struct mbuf *m)
+{
+	struct syncache *sc;
+	struct syncache_head *sch;
+	struct syncache scs;
+	char *s;
+
+	struct ip *ip;
+	ip = mtod(m, struct ip *);
+
+	/*
+	 * Global TCP locks are held because we manipulate the PCB lists
+	 * and create a new socket.
+	 */
+	INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
+	KASSERT((th->th_flags & (TH_RST|TH_ACK|TH_SYN)) == TH_ACK,
+	    ("%s: can handle only ACK", __func__));
+	sc = syncache_lookup(inc, &sch);	/* returns locked sch */
+	SCH_LOCK_ASSERT(sch);
+
+	if (sc == NULL) {
+
+		/*
+		 * There is no syncache entry, so see if this ACK is
+		 * a returning syncookie.  To do this, first:
+		 *  A. See if this socket has had a syncache entry dropped in
+		 *     the past.  We don't want to accept a bogus syncookie
+		 *     if we've never received a SYN.
+		 *  B. check that the syncookie is valid.  If it is, then
+		 *     cobble up a fake syncache entry, and return.
+		 */
+		if (!V_tcp_syncookies) {
+			SCH_UNLOCK(sch);
+			if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+				log(LOG_DEBUG, "%s; %s: Spurious ACK, "
+				    "segment rejected (syncookies disabled)\n",
+				    s, __func__);
+			goto failed;
+		}
+		bzero(&scs, sizeof(scs));
+		sc = syncookie_lookup(inc, sch, &scs, th, to, *lsop);
+		SCH_UNLOCK(sch);
+		if (sc == NULL) {
+			if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+				log(LOG_DEBUG, "%s; %s: Segment failed "
+				    "SYNCOOKIE authentication, segment rejected "
+				    "(probably spoofed)\n", s, __func__);
+			goto failed;
+		}
+	} else {
+		/* Pull out the entry to unlock the bucket row. */
+		TAILQ_REMOVE(&sch->sch_bucket, sc, sc_hash);
+		sch->sch_length--;
+#ifdef TCP_OFFLOAD
+		if (ADDED_BY_TOE(sc)) {
+			struct toedev *tod = sc->sc_tod;
+
+			tod->tod_syncache_removed(tod, sc->sc_todctx);
+		}
+#endif
+		SCH_UNLOCK(sch);
+	}
+
+	/*
+	 * Segment validation:
+	 * ACK must match our initial sequence number + 1 (the SYN|ACK).
+	 */
+	if (th->th_ack != sc->sc_iss + 1) {
+		if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+			log(LOG_DEBUG, "%s; %s: ACK %u != ISS+1 %u, segment "
+			    "rejected\n", s, __func__, th->th_ack, sc->sc_iss);
+		goto failed;
+	}
+
+	/*
+	 * The SEQ must fall in the window starting at the received
+	 * initial receive sequence number + 1 (the SYN).
+	 */
+	if (SEQ_LEQ(th->th_seq, sc->sc_irs) ||
+	    SEQ_GT(th->th_seq, sc->sc_irs + sc->sc_wnd)) {
+		if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+			log(LOG_DEBUG, "%s; %s: SEQ %u != IRS+1 %u, segment "
+			    "rejected\n", s, __func__, th->th_seq, sc->sc_irs);
+		goto failed;
+	}
+
+	if (!(sc->sc_flags & SCF_TIMESTAMP) && (to->to_flags & TOF_TS)) {
+		if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+			log(LOG_DEBUG, "%s; %s: Timestamp not expected, "
+			    "segment rejected\n", s, __func__);
+		goto failed;
+	}
+	/*
+	 * If timestamps were negotiated the reflected timestamp
+	 * must be equal to what we actually sent in the SYN|ACK.
+	 */
+	if ((to->to_flags & TOF_TS) && to->to_tsecr != sc->sc_ts) {
+		if ((s = tcp_log_addrs(inc, th, NULL, NULL)))
+			log(LOG_DEBUG, "%s; %s: TSECR %u != TS %u, "
+			    "segment rejected\n",
+			    s, __func__, to->to_tsecr, sc->sc_ts);
+		goto failed;
+	}
+
+	/*
+	 *  If this is the final ACK of an MP_JOIN handshake, we need to compare
+	 *  the bytes of the hmac sent against those computed locally
+	 */
+	//to->to_mopts.snd_mac
+
+	struct sockaddr_in nam;
+	nam.sin_family = AF_INET;
+	nam.sin_port = th->th_sport;
+	nam.sin_addr = ip->ip_src;
+	nam.sin_len = sizeof(struct sockaddr_in);
+
+	/* debug output for addresses */
+	char buf[128];
+	inet_ntop(AF_INET, &ip->ip_src, buf, sizeof(buf));
+	mp_debug(MPSESSION, 4, 0, "%s: ip header from %s port %d to ",
+	    __func__, buf, ntohs(th->th_sport));
+	inet_ntop(AF_INET, &ip->ip_dst, buf, sizeof(buf));
+	mp_debug(MPSESSION, 4, 0, "%s port %d\n", buf, ntohs(th->th_dport));
+
+	*lsop = syncache_socket_subflow(sc, *lsop, m, &nam);
+
+	if (*lsop == NULL)
+		TCPSTAT_INC(tcps_sc_aborted);
+	else
+		TCPSTAT_INC(tcps_sc_completed);
+
 	if (sc != &scs)
 		syncache_free(sc);
 	return (1);
@@ -1129,8 +1482,9 @@
 	 */
 	so = *lsop;
 	tp = sototcpcb(so);
-	cred = crhold(so->so_cred);
 
+	cred = crhold(so->so_cred);											// doesn't look like it changes the master_tp or socket
+																		// could leave as-is for now.
 #ifdef INET6
 	if ((inc->inc_flags & INC_ISIPV6) &&
 	    (inp->inp_flags & IN6P_AUTOFLOWLABEL))
@@ -1272,7 +1626,7 @@
 	sc->sc_todctx = todctx;
 #endif
 	sc->sc_irs = th->th_seq;
-	sc->sc_iss = arc4random();
+	sc->sc_iss = V_tcp_override_isn > 0 ? V_tcp_override_isn : arc4random();
 	sc->sc_flags = 0;
 	sc->sc_flowlabel = 0;
 
@@ -1337,6 +1691,46 @@
 	if (to->to_flags & TOF_SIGNATURE || ltflags & TF_SIGNATURE)
 		sc->sc_flags |= SCF_SIGNATURE;
 #endif
+	sc->mpc = 0;
+	sc->sent_capable = 0;
+	sc->is_master_subflow = 1;
+
+	/*
+	 * MP SYN packets are either the start of a new connection
+	 * (MP_CAPABLE) or the addition of a subflow to an existing
+	 * connection (MP_JOIN).
+	 */
+	if (to->to_flags & TOF_MPTCP) {
+		if(to->to_mopts.mpo_flags & MPOF_MP_CAPABLE) {
+			sc->sc_flags |= SCF_MPTCP;
+			sc->sc_mp_flags |= MPF_MP_CAPABLE;
+			sc->sc_remote_key = to->to_mopts.remote_key;
+			sc->sc_local_key = mp_generate_local_key();
+			sc->mpc = 1;
+			if (to->to_mopts.mpo_flags & MPOF_USE_CSUM)
+				sc->csum_enabled = 1;
+		} else if(to->to_mopts.mpo_flags & MPOF_JOIN_SYN) {
+			sc->sc_flags |= SCF_MPTCP;
+			sc->sc_mp_flags |= MPF_MP_JOIN;
+			sc->sc_mp_token = to->to_mopts.rcv_token;
+			sc->sc_local_rand = arc4random();
+			sc->sc_remote_rand = to->to_mopts.snd_rnd;
+			sc->sc_local_key = to->to_mopts.local_key;
+			sc->sc_remote_key = to->to_mopts.remote_key;
+
+			uint32_t mac_len = mp_get_hmac(sc->sc_hmac_remote,
+				sc->sc_local_key, sc->sc_remote_key, sc->sc_local_rand,
+				sc->sc_remote_rand);
+
+			if (mac_len == 0) {
+				panic("%s: error computing MAC on JOIN_SYN\n", __func__);
+			}
+
+			sc->is_master_subflow = 0;
+			sc->mpc = 1;
+		}
+	}
+
 	if (to->to_flags & TOF_SACKPERM)
 		sc->sc_flags |= SCF_SACK;
 	if (to->to_flags & TOF_MSS)
@@ -1358,7 +1752,6 @@
 	}
 #endif
 	SCH_UNLOCK(sch);
-
 	/*
 	 * Do a standard 3-way handshake.
 	 */
@@ -1487,11 +1880,11 @@
 	th->th_win = htons(sc->sc_wnd);
 	th->th_urp = 0;
 
+
 	if (sc->sc_flags & SCF_ECN) {
 		th->th_flags |= TH_ECE;
 		TCPSTAT_INC(tcps_ecn_shs);
 	}
-
 	/* Tack on the TCP options. */
 	if ((sc->sc_flags & SCF_NOOPT) == 0) {
 		to.to_flags = 0;
@@ -1508,14 +1901,60 @@
 			to.to_tsecr = sc->sc_tsreflect;
 			to.to_flags |= TOF_TS;
 		}
+		if(sc->sc_flags & SCF_MPTCP) {
+			to.to_mopts.mpo_flags = 0;
+			/* Got a SYN with MP_CAPABLE, so send MP_CAPABLE back */
+			if(sc->sc_mp_flags & MPF_MP_CAPABLE) {
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |=
+						MPOF_CAPABLE_SYN;
+				to.to_mopts.local_key = sc->sc_local_key;
+
+				if (sc->csum_enabled)
+					to.to_mopts.mpo_flags |= MPOF_USE_CSUM;
+			}
+			/* Got a valid MP_JOIN, send MP_JOIN SYN/ACK back */
+			if(sc->sc_mp_flags & MPF_MP_JOIN) {
+				// include addressID?
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |=
+						MPOF_JOIN_SYN;
+				to.to_mopts.to_mpoptlen =
+						MPTCP_SUBLEN_MP_JOIN_SYNACK;
+				to.to_mopts.snd_rnd = sc->sc_local_rand;
+
+				uint8_t digest[20];
+				uint32_t mac_len = mp_get_hmac(digest, sc->sc_local_key,
+				    sc->sc_remote_key, sc->sc_local_rand, sc->sc_remote_rand);
+
+				if (mac_len == 0) {
+					printf("%s: error computing MAC-A\n", __func__);
+					return (ECONNREFUSED); // XXXNJW not really a connrefused...
+				}
+
+				bcopy(digest, sc->sc_hmac_local,
+					sizeof(uint64_t));
+
+				/* We copy the leftmost 64-bits of the HMAC into the option */
+				bcopy(sc->sc_hmac_local, &to.to_mopts.truncated_MAC,
+				    sizeof(uint64_t));
+			}
+		}
 		if (sc->sc_flags & SCF_SACK)
 			to.to_flags |= TOF_SACKPERM;
 #ifdef TCP_SIGNATURE
 		if (sc->sc_flags & SCF_SIGNATURE)
 			to.to_flags |= TOF_SIGNATURE;
 #endif
+		uint64_t mpof_flags = to.to_mopts.mpo_flags;
 		optlen = tcp_addoptions(&to, (u_char *)(th + 1));
 
+		/* If mp_capable was set and is now unset, then it has
+		 * been added as an option on the outgoing SYN/ACK */
+		if ((mpof_flags & MPOF_CAPABLE_SYN) &&
+		    (!to.to_mopts.mpo_flags & MPOF_CAPABLE_SYN))
+			sc->sent_capable = 1;
+
 		/* Adjust headers by option size. */
 		th->th_off = (sizeof(struct tcphdr) + optlen) >> 2;
 		m->m_len += optlen;
@@ -2025,3 +2464,29 @@
 	*pcbs_exported = count;
 	return error;
 }
+
+/* Find inp matching addresses and return locked */
+static inline struct inpcb*
+inp_lookup(struct in_addr *faddr, struct in_addr *laddr, uint16_t fport,
+    uint16_t lport) {
+	struct inpcb *inp = NULL;
+	struct tcpcb *tp = NULL;
+
+	INP_INFO_LOCK_ASSERT(&V_tcbinfo);
+	LIST_FOREACH(inp, &V_tcb, inp_list) {
+		INP_WLOCK(inp);
+		/* Important to skip tcptw structs. */
+		if (!(inp->inp_flags & INP_TIMEWAIT) &&
+			(tp = intotcpcb(inp)) != NULL) {
+				if (inp->inp_faddr.s_addr == faddr->s_addr &&
+					inp->inp_laddr.s_addr == laddr->s_addr &&
+					inp->inp_fport == fport &&
+					inp->inp_lport == lport) {
+					break;
+				}
+		}
+		INP_WUNLOCK(inp);
+	}
+
+	return(inp);
+}
diff -r 1e55e1aa8b7f sys/netinet/tcp_syncache.h
--- a/sys/netinet/tcp_syncache.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_syncache.h	Fri Jul 11 13:32:06 2014 +1000
@@ -41,6 +41,8 @@
 void	 syncache_unreach(struct in_conninfo *, struct tcphdr *);
 int	 syncache_expand(struct in_conninfo *, struct tcpopt *,
 	     struct tcphdr *, struct socket **, struct mbuf *);
+int	 syncache_expand_subflow(struct in_conninfo *, struct tcpopt *,
+	     struct tcphdr *, struct socket **, struct mbuf *);
 void	 syncache_add(struct in_conninfo *, struct tcpopt *,
 	     struct tcphdr *, struct inpcb *, struct socket **, struct mbuf *,
 	     void *, void *);
@@ -77,6 +79,26 @@
 
 	void		*sc_pspare;		/* TCP_SIGNATURE */
 	u_int32_t	sc_spare[2];		/* UTO */
+
+	/* MULTIPATH */
+	uint64_t	sc_ds_iss;	/* initial data sequence num */
+	uint64_t	sc_ds_irs;
+	u_int64_t	sc_remote_key;
+	u_int64_t	sc_local_key;
+	uint32_t 	sc_local_rand;	/* random number for join MAC */
+	uint32_t 	sc_remote_rand;	/* random number for join MAC */
+	uint32_t	sc_mp_token;	/* token for the multipath session */
+
+	uint8_t sc_hmac_local[20];	/* MP_JOIN handshakes use HMACs to ensure */
+	uint8_t sc_hmac_remote[20];	/* that a subflow belongs to connection */
+	char* 	sc_sentmac_ptr;		/* points to HMAC sent in mp_join ACK*/
+
+	uint8_t mpc:1,			/* other end supports mptcp */
+			sent_capable:1,	/* sent mp_cable */
+			csum_enabled:1,
+			is_master_subflow:1;
+	uint8_t		sc_mp_flags;	/* flags for mp subtype */
+	struct	mpcb *t_mpcb;		/* back pointer to multipath cb */
 };
 
 /*
@@ -88,9 +110,19 @@
 						/* MSS is implicit */
 #define SCF_UNREACH	0x10			/* icmp unreachable received */
 #define SCF_SIGNATURE	0x20			/* send MD5 digests */
+#define	SCF_MPTCP	0x40			/* MPTCP capable */
 #define SCF_SACK	0x80			/* send SACK option */
 #define SCF_ECN		0x100			/* send ECN setup packet */
 
+#define	SYNCOOKIE_SECRET_SIZE	16
+#define	SYNCOOKIE_LIFETIME	15		/* seconds */
+
+/*
+ * Flags for sc_mp_flags
+ */
+#define MPF_MP_CAPABLE	0x01
+#define MPF_MP_JOIN		0x02
+
 struct syncache_head {
 	struct mtx	sch_mtx;
 	TAILQ_HEAD(sch_head, syncache)	sch_bucket;
@@ -100,9 +132,6 @@
 	struct tcp_syncache *sch_sc;
 };
 
-#define	SYNCOOKIE_SECRET_SIZE	16
-#define	SYNCOOKIE_LIFETIME	15		/* seconds */
-
 struct syncookie_secret {
 	volatile u_int oddeven;
 	uint8_t key[2][SYNCOOKIE_SECRET_SIZE];
diff -r 1e55e1aa8b7f sys/netinet/tcp_timer.c
--- a/sys/netinet/tcp_timer.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_timer.c	Fri Jul 11 13:32:06 2014 +1000
@@ -47,6 +47,7 @@
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
 #include <sys/systm.h>
+#include <sys/taskqueue.h>
 
 #include <net/if.h>
 #include <net/route.h>
@@ -64,6 +65,7 @@
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
 #include <netinet/tcpip.h>
+#include <netinet/mptcp_var.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
 #endif
@@ -490,6 +492,7 @@
 
 	ostate = tp->t_state;
 #endif
+
 	INP_INFO_RLOCK(&V_tcbinfo);
 	inp = tp->t_inpcb;
 	/*
@@ -525,6 +528,9 @@
 	 * Retransmission timer went off.  Message has not
 	 * been acked within retransmit interval.  Back off
 	 * to a longer retransmit interval and retransmit one segment.
+	 *
+	 * XXXNJW: Do not want to drop the whole connection if there are other
+	 * active subflows. The modifications to do this are temporary.
 	 */
 	if (++tp->t_rxtshift > TCP_MAXRXTSHIFT) {
 		tp->t_rxtshift = TCP_MAXRXTSHIFT;
@@ -551,8 +557,10 @@
 		headlocked = 1;
 		goto out;
 	}
+
 	INP_INFO_RUNLOCK(&V_tcbinfo);
 	headlocked = 0;
+
 	if (tp->t_state == TCPS_SYN_SENT) {
 		/*
 		 * If the SYN was retransmitted, indicate CWND to be
@@ -615,6 +623,17 @@
 		tp->t_rttvar += (tp->t_srtt >> TCP_RTT_SHIFT);
 		tp->t_srtt = 0;
 	}
+
+	/*
+	 * After two RTOs firing, try to re-transmit un-acked data-level segments.
+	 * XXXNJW: want to set up a generic hook so that the rexmit can be handled
+	 * in a more dynamic way.
+	 */
+	// triggered only once. all un-acked segments are enqueued at this time.
+	if (tp->t_state == TCPS_ESTABLISHED && tp->mp_active &&
+	    tp->t_rxtshift == TCP_MAXRXTSHIFT / 6)
+		mp_queue_data_rexmit_unlocked(tp);
+
 	tp->snd_nxt = tp->snd_una;
 	tp->snd_recover = tp->snd_max;
 	/*
diff -r 1e55e1aa8b7f sys/netinet/tcp_timewait.c
--- a/sys/netinet/tcp_timewait.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_timewait.c	Fri Jul 11 13:32:06 2014 +1000
@@ -39,6 +39,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/callout.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
 #include <sys/malloc.h>
@@ -77,6 +78,7 @@
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
 #ifdef INET6
 #include <netinet6/tcp6_var.h>
 #endif
@@ -319,6 +321,15 @@
  */
 	acknow = tp->t_flags & TF_ACKNOW;
 
+	/* XXXNJW a very rough test to prevent socket being released while other
+	 * subflows can still use it. To be removed once a proper shutdown
+	 * implementation is sorted out. clearing the flag that indicates we
+	 * hole the only reference. should ideally be cleared when new subflows
+	 * are added, and set when only one subflow remains. */
+	if (tp->t_mpcb)
+		if (tp->t_mpcb->subflow_cnt != 1)
+			inp->inp_flags &= ~INP_SOCKREF;
+
 	/*
 	 * First, discard tcpcb state, which includes stopping its timers and
 	 * freeing it.  tcp_discardcb() used to also release the inpcb, but
@@ -329,11 +340,14 @@
 	 */
 	tcp_discardcb(tp);
 	so = inp->inp_socket;
-	soisdisconnected(so);
+//	if((so->so_state & SS_ISDISCONNECTED) == 0)
+//		soisdisconnected(so);
 	tw->tw_cred = crhold(so->so_cred);
+
 	SOCK_LOCK(so);
 	tw->tw_so_options = so->so_options;
 	SOCK_UNLOCK(so);
+
 	if (acknow)
 		tcp_twrespond(tw, TH_ACK);
 	inp->inp_ppcb = tw;
@@ -344,17 +358,20 @@
 	 * If the inpcb owns the sole reference to the socket, then we can
 	 * detach and free the socket as it is not needed in time wait.
 	 */
-	if (inp->inp_flags & INP_SOCKREF) {
+	if (inp->inp_flags & INP_SOCKREF &&
+	    (so->so_state & SS_ISDISCONNECTED)) {
 		KASSERT(so->so_state & SS_PROTOREF,
-		    ("tcp_twstart: !SS_PROTOREF"));
+			("tcp_twstart: !SS_PROTOREF"));
 		inp->inp_flags &= ~INP_SOCKREF;
 		INP_WUNLOCK(inp);
 		ACCEPT_LOCK();
 		SOCK_LOCK(so);
 		so->so_state &= ~SS_PROTOREF;
+		printf("%s: calling sofree with discarded tp %p\n", __func__, tp);
 		sofree(so);
 	} else
 		INP_WUNLOCK(inp);
+
 }
 
 #if 0
diff -r 1e55e1aa8b7f sys/netinet/tcp_usrreq.c
--- a/sys/netinet/tcp_usrreq.c	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_usrreq.c	Fri Jul 11 13:32:06 2014 +1000
@@ -46,10 +46,17 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/limits.h>
+#include <sys/endian.h>
+#include <sys/lock.h>
 #include <sys/malloc.h>
+#include <sys/mutex.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
 #include <sys/mbuf.h>
+#include <sys/queue.h>
+#include <sys/taskqueue.h>
+
 #ifdef INET6
 #include <sys/domain.h>
 #endif /* INET6 */
@@ -80,10 +87,13 @@
 #include <netinet6/ip6_var.h>
 #include <netinet6/scope6_var.h>
 #endif
+
 #include <netinet/tcp_fsm.h>
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
 #include <netinet/tcpip.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
@@ -171,6 +181,7 @@
 
 	tp = intotcpcb(inp);
 
+
 	if (inp->inp_flags & INP_TIMEWAIT) {
 		/*
 		 * There are two cases to handle: one in which the time wait
@@ -384,6 +395,7 @@
 }
 #endif /* INET */
 
+
 #ifdef INET6
 static int
 tcp6_usr_listen(struct socket *so, int backlog, struct thread *td)
@@ -436,18 +448,24 @@
  * Enter SYN_SENT state, and mark socket as connecting.
  * Start keep-alive timer, and seed output sequence space.
  * Send initial segment on connection.
+ *
+ *
  */
 static int
 tcp_usr_connect(struct socket *so, struct sockaddr *nam, struct thread *td)
 {
 	int error = 0;
 	struct inpcb *inp;
+	struct mpcb *mp;
 	struct tcpcb *tp = NULL;
 	struct sockaddr_in *sinp;
 
+	printf("%s: entered\n", __func__);
+
 	sinp = (struct sockaddr_in *)nam;
 	if (nam->sa_len != sizeof (*sinp))
 		return (EINVAL);
+
 	/*
 	 * Must disallow TCP ``connections'' to multicast addresses.
 	 */
@@ -465,10 +483,24 @@
 		error = EINVAL;
 		goto out;
 	}
-	tp = intotcpcb(inp);
+
+	if ((so->so_state & SS_ISCONNECTED) == 0)  {
+		tp = intotcpcb(inp);
+		if ((error = tcp_connect(tp, nam, td)) != 0)
+			goto out;
+	} else {
+		INP_WUNLOCK(inp);
+		/* Aquire mp and (tp, inp) for most recently added subflow */
+		mp = sototcpcb(so)->t_mpcb;
+		tp = (SLIST_FIRST(&mp->tp_list));
+		inp = tp->t_inpcb;
+
+		KASSERT(tp != NULL, ("tcp_usr_connect: subflow tp == NULL"));
+		if ((error = tcp_subflow_connect(tp, nam, td)) != 0)
+			goto out;
+	}
+	
 	TCPDEBUG1();
-	if ((error = tcp_connect(tp, nam, td)) != 0)
-		goto out;
 #ifdef TCP_OFFLOAD
 	if (registered_toedevs > 0 &&
 	    (so->so_options & SO_NO_OFFLOAD) == 0 &&
@@ -477,6 +509,7 @@
 #endif
 	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
 	error = tcp_output(tp);
+
 out:
 	TCPDEBUG2(PRU_CONNECT);
 	INP_WUNLOCK(inp);
@@ -484,6 +517,7 @@
 }
 #endif /* INET */
 
+
 #ifdef INET6
 static int
 tcp6_usr_connect(struct socket *so, struct sockaddr *nam, struct thread *td)
@@ -583,29 +617,90 @@
 static int
 tcp_usr_disconnect(struct socket *so)
 {
-	struct inpcb *inp;
-	struct tcpcb *tp = NULL;
+	int close_socket = 1;
 	int error = 0;
+	struct inpcb *sf_inp;
+	struct mpcb *mp;
+	struct tcpcb *sf_tp;
+
+	sf_tp = sototcpcb(so);
+	mp = sf_tp->t_mpcb;
 
 	TCPDEBUG0;
 	INP_INFO_WLOCK(&V_tcbinfo);
-	inp = sotoinpcb(so);
-	KASSERT(inp != NULL, ("tcp_usr_disconnect: inp == NULL"));
-	INP_WLOCK(inp);
-	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
-		error = ECONNRESET;
-		goto out;
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		sf_inp = sf_tp->t_inpcb;
+		KASSERT(sf_inp != NULL, ("tcp_usr_disconnect: tp %p inp == NULL", sf_tp));
+
+		printf("%s: start close on tp %p\n", __func__, sf_tp);
+
+		INP_WLOCK(sf_inp);
+		if (sf_inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+			error = ECONNRESET;
+			TCPDEBUG2(PRU_DISCONNECT);
+			INP_WUNLOCK(sf_inp);
+			goto out;
+		}
+		sf_tp = intotcpcb(sf_inp);
+
+		/* XXXNJW: need to overhaul how multiple subflows are being closed, as
+		 * it gets a bit messy if subflows shutdown with different paths thru
+		 * shutdown states.
+		 * Only need to call tcp_disconnect once.
+		 * close_socket == 1: Call tcp_disconnect, which flushed buffers,
+		 * calls sowakup, calls tcp_usrclosed, tcp_output.
+		 * close_socket == 0: Just set the state and call tcp_output to
+		 * issue the FIN */
+		if (close_socket) {
+			printf("%s: first loop through tp list, tp %p\n", __func__, sf_tp);
+			TCPDEBUG1();
+			tcp_disconnect(sf_tp);
+			TCPDEBUG2(PRU_DISCONNECT);
+			close_socket = 0;
+		} else {
+			printf("%s: second loop through tp list, tp %p\n", __func__, sf_tp);
+			tcp_usrclosed(sf_tp);
+			if (!(sf_inp->inp_flags & INP_DROPPED))
+				tcp_output(sf_tp);
+		}
+		if (sf_tp->t_inpcb)
+			INP_WUNLOCK(sf_inp);
 	}
-	tp = intotcpcb(inp);
-	TCPDEBUG1();
-	tcp_disconnect(tp);
 out:
-	TCPDEBUG2(PRU_DISCONNECT);
-	INP_WUNLOCK(inp);
 	INP_INFO_WUNLOCK(&V_tcbinfo);
 	return (error);
 }
 
+//static int
+//tcp_usr_disconnect(struct socket *so)
+//{
+//	struct inpcb *inp;
+//	struct tcpcb *tp = NULL;
+//	int error = 0;
+//
+//	TCPDEBUG0;
+//	INP_INFO_WLOCK(&V_tcbinfo);
+//	inp = sotoinpcb(so);
+//	KASSERT(inp != NULL, ("tcp_usr_disconnect: inp == NULL"));
+//	INP_WLOCK(inp);
+//	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+//		error = ECONNRESET;
+//		goto out;
+//	}
+//	tp = intotcpcb(inp);
+//
+//	printf("%s: calling tcp_disco. tp %p\n", __func__, tp);
+//
+//	TCPDEBUG1();
+//	tcp_disconnect(tp);
+//out:
+//	TCPDEBUG2(PRU_DISCONNECT);
+//	INP_WUNLOCK(inp);
+//	INP_INFO_WUNLOCK(&V_tcbinfo);
+//	return (error);
+//}
+
+
 #ifdef INET
 /*
  * Accept a connection.  Essentially all the work is done at higher levels;
@@ -765,8 +860,11 @@
 		tcp_offload_rcvd(tp);
 	else
 #endif
+
+	/* the app has read in data and an ACK/DACK will now
+	 * be scheduled for the next output.
+	 */
 	tcp_output(tp);
-
 out:
 	TCPDEBUG2(PRU_RCVD);
 	INP_WUNLOCK(inp);
@@ -813,6 +911,57 @@
 	isipv6 = nam && nam->sa_family == AF_INET6;
 #endif /* INET6 */
 	tp = intotcpcb(inp);
+
+	/* temporary "round-robin" -like behaviour. Distributes pru_usr_send calls
+	 * to tcp_output between the available subflows. Works for two subflows. */
+	if (tp->mp_active) {
+		struct tcpcb *sf_tp;
+		struct mpcb *mp = tp->t_mpcb;
+
+		/* Not the last segment, and more than one subflow */
+		if (!(flags & PRUS_EOF) && (mp->subflow_cnt > 1)) {
+			INP_INFO_WLOCK(&V_tcbinfo);
+
+			/* Subflow must be waiting to send data, have room for at least
+			 * on segment in the send window and not be in retransmit. If the
+			 * flow is in rexmit then the tcp timers will take care of calling
+			 * tcp_output and sending data. */
+			SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+				if ((sf_tp->sf_flags & SFF_SEND_WAIT)
+					&& (sf_tp->snd_cwnd >= sf_tp->t_maxseg)
+				    && (sf_tp->t_rxtshift == 0)) {
+					/* don't send if in time_wait, not established  */
+					if ((sf_tp->t_inpcb->inp_flags & (INP_TIMEWAIT | INP_DROPPED))
+						|| (sf_tp->t_state != TCPS_ESTABLISHED)) {
+						tp->sf_flags &= ~SFF_SEND_WAIT;
+						continue;
+					}
+					/* unlock the current inp, and re-assign to sf_inp */
+					INP_WUNLOCK(inp);
+					inp = sf_tp->t_inpcb;
+					INP_WLOCK(inp);
+					if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {			// should be checked above anyway
+						printf("%s: found a subflow in timewait - break\n",
+						    __func__);
+						kdb_break();
+					}
+					tp = sf_tp;
+					/* assigned tp to send on, break loop */
+					break;
+				} else {
+					/* Can use this subflow next time through */
+					sf_tp->sf_flags |= SFF_SEND_WAIT;
+				}
+			}
+			INP_INFO_WUNLOCK(&V_tcbinfo);
+
+			/* Dont want to send from this sf next time */
+			tp->sf_flags &= ~SFF_SEND_WAIT;
+		}
+
+	}
+	/* End "Round-Robin" allocation */
+
 	TCPDEBUG1();
 	if (control) {
 		/* TCP doesn't do control messages (rights, creds, etc) */
@@ -976,25 +1125,30 @@
 	INP_INFO_WLOCK(&V_tcbinfo);
 	INP_WLOCK(inp);
 	KASSERT(inp->inp_socket != NULL,
-	    ("tcp_usr_close: inp_socket == NULL"));
+		("tcp_usr_close: inp_socket == NULL"));
 
 	/*
 	 * If we still have full TCP state, and we're not dropped, initiate
 	 * a disconnect.
 	 */
 	if (!(inp->inp_flags & INP_TIMEWAIT) &&
-	    !(inp->inp_flags & INP_DROPPED)) {
+		!(inp->inp_flags & INP_DROPPED)) {
 		tp = intotcpcb(inp);
+
+		printf("%s: calling tcp_disco. tp %p\n", __func__, tp);
+
 		TCPDEBUG1();
 		tcp_disconnect(tp);
 		TCPDEBUG2(PRU_CLOSE);
 	}
+
 	if (!(inp->inp_flags & INP_DROPPED)) {
 		SOCK_LOCK(so);
 		so->so_state |= SS_PROTOREF;
 		SOCK_UNLOCK(so);
 		inp->inp_flags |= INP_SOCKREF;
 	}
+
 	INP_WUNLOCK(inp);
 	INP_INFO_WUNLOCK(&V_tcbinfo);
 }
@@ -1105,6 +1259,8 @@
 	u_short lport;
 	int error;
 
+	struct mpcb *mp;
+
 	INP_WLOCK_ASSERT(inp);
 	INP_HASH_WLOCK(&V_tcbinfo);
 
@@ -1144,7 +1300,117 @@
 
 	soisconnecting(so);
 	TCPSTAT_INC(tcps_connattempt);
+	tp->t_state = TCPS_SYN_SENT;
+	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
+	tp->iss = tcp_new_isn(tp);
+	tcp_sendseqinit(tp);
+
+	/*
+	 * Create new multipath control block. for the connection
+	 * inp and socket are set in mp_newmpcb
+	 */
+	mp = mp_newmpcb(inp);
+	mp->master_tcpcb = tp;
+
+	/* add subflow tp to tp_list */
+	SLIST_INSERT_HEAD(&mp->tp_list, mp->master_tcpcb, next_subflow);
+
+	mp->subflow_cnt = 1;
+	tp->t_mpcb = mp;
+
+	/* should have a sysctl to check if we want to enable */
+	tp->mp_enabled = 1;
+	tp->infinite_map = 1;
+
+	/* XXXNJW: temporary - disable multipath on local connections */
+	if (inp->inp_faddr.s_addr == inp->inp_laddr.s_addr)
+		tp->mp_enabled = 0;
+
+	/* XXXNJW: cheap check to disable multipath for localhost connections */
+	struct sockaddr_in localhost_addr;
+	inet_pton(AF_INET, "127.0.0.1", &localhost_addr.sin_addr);
+	if (inp->inp_laddr.s_addr == localhost_addr.sin_addr.s_addr)
+		tp->mp_enabled = 0;
+
+	STAILQ_INSERT_TAIL(&mp->sf_reass_queues_list, tp->t_segq, tqe_next_head);
+
+	if (tp->t_mpcb->mp_state == MPS_CLOSED) {
+		mp_process_local_key(tp->t_mpcb, mp_generate_local_key());
+
+		/* Always start in direct mapping mode */
+		tp->t_mpcb->ds_map_max = tp->t_mpcb->ds_map_min = tp->t_mpcb->ds_snd_una = tp->iss + 1;
+		tp->t_mpcb->mp_state = MPS_SYN_SENT;
+
+	}
+
+	return 0;
+
+out:
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+	return (error);
+}
+#endif /* INET */
+
+
+/* XXXNJW: Refactoring needed to tidy and reduce code duplication.
+ */
+int
+tcp_subflow_connect(struct tcpcb *tp, struct sockaddr *nam, struct thread *td)
+{
+	struct inpcb *inp = tp->t_inpcb, *oinp;
+	struct in_addr laddr;
+	u_short lport;
+	int error = 0;
+
+	INP_WLOCK_ASSERT(inp);
+	INP_HASH_WLOCK(&V_tcbinfo);
+
+	/*
+	 * If lport == 0., set an ephemeral port number. otherise we just
+	 * add directly into hash list.
+	 */
+	if (inp->inp_lport == 0) {
+		error = in_pcbbind(inp, (struct sockaddr *)0, td->td_ucred);
+	} else {
+		error = in_pcbinshash(inp);
+		mp_debug(MPSESSION, 4, 0, "%s: in_pcbhash error %d\n", __func__, error);
+	}
+	if (error)
+		goto out;
+
+	/*
+	 * Cannot simply call in_pcbconnect, because there might be an
+	 * earlier incarnation of this same connection still in
+	 * TIME_WAIT state, creating an ADDRINUSE error.
+	 */
+	laddr = inp->inp_laddr;
+	lport = inp->inp_lport;
+	error = in_pcbconnect_setup(inp, nam, &laddr.s_addr, &lport,
+	    &inp->inp_faddr.s_addr, &inp->inp_fport, &oinp, td->td_ucred);
+
+	if (error && oinp == NULL) {
+		goto out;
+	}
+	if (oinp) {
+		error = EADDRINUSE;
+		goto out;
+	}
+	inp->inp_laddr = laddr;
+	in_pcbrehash(inp);
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+
+	/*
+	 * Compute window scaling to request:
+	 * Scale to fit into sweet spot.  See tcp_syncache.c.
+	 * XXX: This should move to tcp_output().
+	 */
+	while (tp->request_r_scale < TCP_MAX_WINSHIFT &&
+	    (TCP_MAXWIN << tp->request_r_scale) < sb_max)
+		tp->request_r_scale++;
+
+	TCPSTAT_INC(tcps_connattempt);
 	tcp_state_change(tp, TCPS_SYN_SENT);
+	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
 	tp->iss = tcp_new_isn(tp);
 	tcp_sendseqinit(tp);
 
@@ -1154,7 +1420,6 @@
 	INP_HASH_WUNLOCK(&V_tcbinfo);
 	return (error);
 }
-#endif /* INET */
 
 #ifdef INET6
 static int
@@ -1616,8 +1881,9 @@
 static int
 tcp_attach(struct socket *so)
 {
+	struct inpcb *inp;
 	struct tcpcb *tp;
-	struct inpcb *inp;
+
 	int error;
 
 	if (so->so_snd.sb_hiwat == 0 || so->so_rcv.sb_hiwat == 0) {
@@ -1627,6 +1893,7 @@
 	}
 	so->so_rcv.sb_flags |= SB_AUTOSIZE;
 	so->so_snd.sb_flags |= SB_AUTOSIZE;
+	so->so_snd.sb_flags |= SB_DEFERACCT;
 	INP_INFO_WLOCK(&V_tcbinfo);
 	error = in_pcballoc(so, &V_tcbinfo);
 	if (error) {
@@ -1642,6 +1909,7 @@
 	else
 #endif
 	inp->inp_vflag |= INP_IPV4;
+
 	tp = tcp_newtcpcb(inp);
 	if (tp == NULL) {
 		in_pcbdetach(inp);
@@ -1656,6 +1924,41 @@
 }
 
 /*
+ * Attach TCP protocol to subflow inp, allocating
+ * buffer space, and entering LISTEN state.
+ */
+int
+tcp_subflow_attach(struct socket *so, struct inpcb *inp, struct mpcb *mp)
+{
+	struct tcpcb *tp;
+
+	INP_WLOCK_ASSERT(inp);
+#ifdef INET6
+	if (inp->inp_vflag & INP_IPV6PROTO) {
+		inp->inp_vflag |= INP_IPV6;
+		inp->in6p_hops = -1;	/* use kernel default */
+	}
+	else
+#endif
+	inp->inp_vflag |= INP_IPV4;
+
+	tp = tcp_newtcpcb(inp);
+
+	if (tp == NULL) {
+		return (ENOBUFS);
+	}
+	/* Add sf to list of subflow blocks */
+	SLIST_INSERT_HEAD(&mp->tp_list, tp, next_subflow);
+	tp->t_mpcb = mp;
+
+	/* Subflows only join mp-enabled sessions */
+	tp->mp_enabled = 1;
+
+	tp->t_state = TCPS_CLOSED;
+	return 0;
+}
+
+/*
  * Initiate (or continue) disconnect.
  * If embryonic state, just send reset (once).
  * If in ``let data drain'' option and linger null, just drop.
@@ -1677,19 +1980,24 @@
 	 * socket is still open.
 	 */
 	if (tp->t_state < TCPS_ESTABLISHED) {
+		printf("%s: closing tp that didn't reach established %p\n", __func__,
+		    tp);
 		tp = tcp_close(tp);
-		KASSERT(tp != NULL,
-		    ("tcp_disconnect: tcp_close() returned NULL"));
+//		KASSERT(tp != NULL,
+//		    ("tcp_disconnect: tcp_close() returned NULL"));
 	} else if ((so->so_options & SO_LINGER) && so->so_linger == 0) {
 		tp = tcp_drop(tp, 0);
 		KASSERT(tp != NULL,
 		    ("tcp_disconnect: tcp_drop() returned NULL"));
 	} else {
+		printf("%s: about to call so is disco with tp %p\n", __func__, tp);		//needs a check for how many subflows?
 		soisdisconnecting(so);
 		sbflush(&so->so_rcv);
 		tcp_usrclosed(tp);
-		if (!(inp->inp_flags & INP_DROPPED))
+		if (!(inp->inp_flags & INP_DROPPED)) {
+			printf("%s: call tcp_output %p\n", __func__, tp);
 			tcp_output(tp);
+		}
 	}
 }
 
@@ -1719,12 +2027,15 @@
 	case TCPS_CLOSED:
 		tcp_state_change(tp, TCPS_CLOSED);
 		tp = tcp_close(tp);
-		/*
-		 * tcp_close() should never return NULL here as the socket is
-		 * still open.
-		 */
-		KASSERT(tp != NULL,
-		    ("tcp_usrclosed: tcp_close() returned NULL"));
+//		/*
+//		 * tcp_close() should never return NULL here as the socket is
+//		 * still open.
+//		 */
+//		KASSERT(tp != NULL,
+//		    ("tcp_usrclosed: tcp_close() returned NULL"));
+		/* A null return is okay as this is a valid return when we have
+		 * multiple subflows (i.e. a subflow can be detached and we still
+		 * could have other subflows that are using the open socket. */
 		break;
 
 	case TCPS_SYN_SENT:
@@ -1733,22 +2044,29 @@
 		break;
 
 	case TCPS_ESTABLISHED:
-		tcp_state_change(tp, TCPS_FIN_WAIT_1);
+		tcp_state_change(tp, TCPS_FIN_WAIT_1); //XXXNJW should be set after all DS-level is acked
 		break;
 
 	case TCPS_CLOSE_WAIT:
 		tcp_state_change(tp, TCPS_LAST_ACK);
 		break;
 	}
-	if (tp->t_state >= TCPS_FIN_WAIT_2) {
-		soisdisconnected(tp->t_inpcb->inp_socket);
-		/* Prevent the connection hanging in FIN_WAIT_2 forever. */
-		if (tp->t_state == TCPS_FIN_WAIT_2) {
-			int timeout;
+	/* if the tp hasn't been detatched */
+	if(tp) {
+		if (tp->t_state >= TCPS_FIN_WAIT_2) {
+			printf("%s: calling soisdisco \n", __func__);
+			if (tp->t_mpcb == NULL ||
+				(tp->t_mpcb != NULL && tp->t_mpcb->subflow_cnt == 1))
+					soisdisconnected(tp->t_inpcb->inp_socket);
 
-			timeout = (tcp_fast_finwait2_recycle) ? 
-			    tcp_finwait2_timeout : TP_MAXIDLE(tp);
-			tcp_timer_activate(tp, TT_2MSL, timeout);
+			/* Prevent the connection hanging in FIN_WAIT_2 forever. */
+			if (tp->t_state == TCPS_FIN_WAIT_2) {
+				int timeout;
+
+				timeout = (tcp_fast_finwait2_recycle) ?
+					tcp_finwait2_timeout : TP_MAXIDLE(tp);
+				tcp_timer_activate(tp, TT_2MSL, timeout);
+			}
 		}
 	}
 }
@@ -1949,7 +2267,7 @@
 
 	db_print_indent(indent);
 	db_printf("t_segq first: %p   t_segqlen: %d   t_dupacks: %d\n",
-	   LIST_FIRST(&tp->t_segq), tp->t_segqlen, tp->t_dupacks);
+	   TAILQ_FIRST(tp->t_segq), tp->t_segqlen, tp->t_dupacks);
 
 	db_print_indent(indent);
 	db_printf("tt_rexmt: %p   tt_persist: %p   tt_keep: %p\n",
diff -r 1e55e1aa8b7f sys/netinet/tcp_var.h
--- a/sys/netinet/tcp_var.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/netinet/tcp_var.h	Fri Jul 11 13:32:06 2014 +1000
@@ -46,14 +46,21 @@
 
 #endif /* _KERNEL */
 
+/* Subflow send buffer mapping queue entry */
+struct sfsendmap_qent {
+	TAILQ_ENTRY(sfsendmap_qent) sfmap_q;
+	int send_chars;
+	void *sendbuf_idx;
+};
+
 /* TCP segment queue entry */
 struct tseg_qent {
-	LIST_ENTRY(tseg_qent) tqe_q;
+	TAILQ_ENTRY(tseg_qent) tqe_q;
+	uint64_t tqe_dsn;		/* The DSN for this entry */
 	int	tqe_len;		/* TCP segment data length */
 	struct	tcphdr *tqe_th;		/* a pointer to tcp header */
 	struct	mbuf	*tqe_m;		/* mbuf contains packet */
 };
-LIST_HEAD(tsegqe_head, tseg_qent);
 
 struct sackblk {
 	tcp_seq start;		/* start seq no. of sack block */
@@ -95,21 +102,53 @@
 #define ND6_HINT(tp)
 #endif
 
+/* Used to map outgoing and incoming segments. Maintained in
+ * t_txmaps and t_rxmaps lists. mbuf_start and mbuf_offset
+ * are not used in the rx context.
+ */
+struct ds_map {
+	TAILQ_ENTRY(ds_map) sf_ds_map_next;
+	TAILQ_ENTRY(ds_map) mp_ds_map_next;
+	TAILQ_ENTRY(ds_map) mp_dup_map_next;
+	TAILQ_ENTRY(ds_map) rxmit_map_next;
+	uint64_t ds_map_start;	/* starting DSN of mapping */
+	uint32_t ds_map_len;	/* length of data sequence mapping */
+	uint32_t ds_map_offset;	/* bytes sent from mapping */
+	tcp_seq	sf_seq_start; /* starting tcp seq num of mapping */
+	struct tcpcb *sf_tp; /* pointer to the sf that this was created for */		// might remove later
+//	uint64_t map_una;		/* bytes sent but unacknowledged in map */
+	uint16_t ds_map_csum;	/* csum of dss psuedo-header and all mapping data */
+	struct mbuf* mbuf_start; /* mbuf in send sockbuf in which this mappings starts */
+	u_int	mbuf_offset;	/* offset into mbuf where data starts */
+	uint32_t flags;
+};
+TAILQ_HEAD(dsmapq_head, ds_map);
+
+/* for unmapped segments received */
+TAILQ_HEAD(sf_unmapped_queue_head, tseg_qent);
+
+
 /*
  * Tcp control block, one per tcp; fields:
  * Organized for 16 byte cacheline efficiency.
  */
 struct tcpcb {
-	struct	tsegqe_head t_segq;	/* segment reassembly queue */
 	void	*t_pspare[2];		/* new reassembly queue */
 	int	t_segqlen;		/* segment reassembly queue length */
 	int	t_dupacks;		/* consecutive dup acks recd */
 
+	struct sf_reass_queue_head *t_segq;	/* points to head of reass queue */
+	struct sf_unmapped_queue_head *t_unmapped_segq; /* unmapped segments buffer */
+
 	struct tcp_timer *t_timers;	/* All the TCP timers in one struct */
 
 	struct	inpcb *t_inpcb;		/* back pointer to internet pcb */
+	struct	mpcb *t_mpcb;		/* back pointer to multipath cb */
+
 	int	t_state;		/* state of this connection */
+	int sf_state;		/* mp state for slave subflows */
 	u_int	t_flags;
+	u_int	sf_flags;	/* flags for this subflow */
 
 	struct	vnet *t_vnet;		/* back pointer to parent vnet */
 
@@ -194,6 +233,7 @@
 	int	t_rttlow;		/* smallest observerved RTT */
 	u_int32_t	rfbuf_ts;	/* recv buffer autoscaling timestamp */
 	int	rfbuf_cnt;		/* recv buffer autoscaling byte count */
+	//struct toe_usrreqs *t_tu;	/* offload operations vector */
 	struct toedev	*tod;		/* toedev handling this connection */
 	int	t_sndrexmitpack;	/* retransmit packets sent */
 	int	t_rcvoopack;		/* out-of-order packets received */
@@ -213,6 +253,25 @@
 	uint32_t t_ispare[8];		/* 5 UTO, 3 TBD */
 	void	*t_pspare2[4];		/* 1 TCP_SIGNATURE, 3 TBD */
 	uint64_t _pad[6];		/* 6 TBD (1-2 CC/RTT?) */
+
+	/* MULTIPATH */
+	uint8_t	mp_enabled:1,	/* try to use mptcp (sysctl is enabled) */
+			mp_active:1,	/* mp session is active on this flow */
+			mpc:1,			/* other end supports mptcp */
+			sent_capable:1, /* sent mp_cable */
+			infinite_map:1;	/* are we in infinite map mode */
+
+	struct dsmapq_head t_txdsmaps;	/* data sequence map outgoing */
+	struct dsmapq_head t_rxdsmaps;	/* data sequence map incoming */
+	u_long	init_cwnd;	/* establish mptcp once this window fills */
+	uint32_t local_rand;	/* random number for join HMAC */
+	uint32_t remote_rand;	/* random number for join HMAC */
+	uint32_t mp_conn_token;	/* mp session this belongs to */
+	SLIST_ENTRY(tcpcb) next_subflow;
+
+	uint8_t 	hmac_local[20];	/* MP_JOIN handshakes use HMACs to ensure */
+	uint8_t 	hmac_remote[20];	/* that a subflow belongs to connection */
+	uint64_t	hmac_trun_remote; /* mp_join sender truncated mac */
 };
 
 /*
@@ -232,6 +291,7 @@
 #define	TF_NEEDFIN	0x000800	/* send FIN (implicit state) */
 #define	TF_NOPUSH	0x001000	/* don't push */
 #define	TF_PREVVALID	0x002000	/* saved values for bad rxmit valid */
+#define	TF_MPTCP		0x004000	/* MPTCP options present */
 #define	TF_MORETOCOME	0x010000	/* More data to be appended to sock */
 #define	TF_LQ_OVERFLOW	0x020000	/* listen queue overflow */
 #define	TF_LASTIDLE	0x040000	/* connection was previously idle */
@@ -247,6 +307,7 @@
 #define	TF_ECN_SND_ECE	0x10000000	/* ECN ECE in queue */
 #define	TF_CONGRECOVERY	0x20000000	/* congestion recovery mode */
 #define	TF_WASCRECOVERY	0x40000000	/* was in congestion recovery */
+#define	TF_NEED_DFIN	0x80000000	/* send mptcp DFIN */
 
 #define	IN_FASTRECOVERY(t_flags)	(t_flags & TF_FASTRECOVERY)
 #define	ENTER_FASTRECOVERY(t_flags)	t_flags |= TF_FASTRECOVERY
@@ -260,7 +321,19 @@
 #define	ENTER_RECOVERY(t_flags) t_flags |= (TF_CONGRECOVERY | TF_FASTRECOVERY)
 #define	EXIT_RECOVERY(t_flags) t_flags &= ~(TF_CONGRECOVERY | TF_FASTRECOVERY)
 
-#define	BYTES_THIS_ACK(tp, th)	(th->th_ack - tp->snd_una)
+#define	OURSYNISACKED(tp, th)												\
+(((tp)->t_state == TCPS_SYN_SENT) && ((th)->th_ack == (tp)->iss + 1) ? 1 : 0)
+
+#define	OURFINISACKED(tp, th)												\
+(((tp)->t_flags & TF_SENTFIN) && ((th)->th_ack == (tp)->snd_max) ? 1 : 0)
+
+#define	FAKE_BYTES_THIS_ACK(tp, th)											\
+	(OURSYNISACKED(tp, th) + OURFINISACKED(tp, th))
+
+#define	BYTES_THIS_ACK(tp, th)												\
+((tp)->snd_una <= (th)->th_ack ?											\
+	((th)->th_ack - (tp)->snd_una - FAKE_BYTES_THIS_ACK(tp, th)) :			\
+	((UINT_MAX - (tp)->snd_una) + 1 + (th)->th_ack - FAKE_BYTES_THIS_ACK(tp, th)))
 
 /*
  * Flags for the t_oobflags field.
@@ -283,6 +356,41 @@
 #define	TCP_SIG_SPI	0x1000
 #endif /* TCP_SIGNATURE */
 
+struct mpopt {
+	uint64_t	mpo_flags;		/* flags for this option */
+	uint8_t		to_mpsubtype;	/* the mptcp option subtype */
+	uint8_t		to_mpoptlen;	/* length of the mptcp option for this segment */
+	uint32_t	add_addr_mask;	/* Bit mask of addresses that have been advertised */
+	uint32_t	join_mask;		/* Addresses we have sent joins for */
+
+	/* KEYS */
+	uint64_t	local_key;	/* locally generated key */
+	uint64_t	remote_key;	/* key generated by remote host */
+
+	/* MP_JOIN */
+	uint32_t	rcv_token;	/* mp_join receive token */
+	uint32_t	snd_rnd;	/* mp_join sender random number */
+	uint64_t	snd_trc_mac;	/* mp_join sender truncated mac */
+
+	/* MP_ADD */
+	struct sockaddr_storage	new_addr;	/* a new address */
+
+	/* DSS */
+	uint64_t	data_ack_num;	/* data ack value */
+	uint64_t	data_seq_num;	/* data sequence number (only 64-bit support for now) */
+	uint64_t	truncated_MAC;	/* truncated (upper 64-bits) MAC for MP_JOIN */
+	uint32_t	sub_seq_num;	/* subflow sequence number */
+	uint16_t	dss_data_len;	/* Data-level length 0 means no DSN */
+	uint16_t	dss_csum;	/* checksum */
+	uint16_t	*dss_data_len_p; /* Pointer to data length in option char */
+	uint8_t		*dss_opts_f_p;	/* pointer to dss flags */
+
+	int		optlen;			/* use this to identify types in segment processing */
+	int		address_count;	/* addresses available for this session */
+	uint8_t	addr_id;		/* address id, used in mp_join */
+	char	*snd_mac;	/* senders MAC, 160-bit sha */
+};
+
 /*
  * Structure to hold TCP options that are only used during segment
  * processing (in tcp_input), but not held in the tcpcb.
@@ -299,7 +407,8 @@
 #define	TOF_TS		0x0010		/* timestamp */
 #define	TOF_SIGNATURE	0x0040		/* TCP-MD5 signature option (RFC2385) */
 #define	TOF_SACK	0x0080		/* Peer sent SACK option */
-#define	TOF_MAXOPT	0x0100
+#define	TOF_MPTCP	0x0100		/* MPTCP options present */
+#define	TOF_MAXOPT	0x0200
 	u_int32_t	to_tsval;	/* new timestamp */
 	u_int32_t	to_tsecr;	/* reflected timestamp */
 	u_char		*to_sacks;	/* pointer to the first SACK blocks */
@@ -308,6 +417,7 @@
 	u_int8_t	to_wscale;	/* window scaling */
 	u_int8_t	to_nsacks;	/* number of SACK blocks */
 	u_int32_t	to_spare;	/* UTO */
+	struct	mpopt	to_mopts;	/* multipath options */
 };
 
 /*
@@ -612,6 +722,8 @@
 VNET_DECLARE(int, path_mtu_discovery);
 VNET_DECLARE(int, tcp_do_rfc3465);
 VNET_DECLARE(int, tcp_abc_l_var);
+VNET_DECLARE(unsigned int, tcp_override_isn);
+#define	V_tcp_override_isn	VNET(tcp_override_isn)
 #define	V_tcb			VNET(tcb)
 #define	V_tcbinfo		VNET(tcbinfo)
 #define	V_tcp_mssdflt		VNET(tcp_mssdflt)
@@ -638,6 +750,15 @@
 VNET_DECLARE(struct hhook_head *, tcp_hhh[HHOOK_TCP_LAST + 1]);
 #define	V_tcp_hhh		VNET(tcp_hhh)
 
+VNET_DECLARE(int, tcp_do_mptcp);	/* MPTCP enabled/disabled */
+#define	V_tcp_do_mptcp		VNET(tcp_do_mptcp)
+
+/*
+ * XXXNJW: use zone allocator. Potentially could have many maps
+ * being alloc'd and dealloc'd if using one map per packet
+ */
+MALLOC_DECLARE(M_DSSMAP);
+
 int	 tcp_addoptions(struct tcpopt *, u_char *);
 int	 tcp_ccalgounload(struct cc_algo *unload_algo);
 struct tcpcb *
@@ -665,6 +786,7 @@
 int	 tcp_reass(struct tcpcb *, struct tcphdr *, int *, struct mbuf *);
 void	 tcp_reass_init(void);
 void	 tcp_reass_flush(struct tcpcb *);
+void	 tcp_reass_release_element(struct tseg_qent *q);
 #ifdef VIMAGE
 void	 tcp_reass_destroy(void);
 #endif
@@ -726,13 +848,29 @@
 void	 tcp_clean_sackreport(struct tcpcb *tp);
 void	 tcp_sack_adjust(struct tcpcb *tp);
 struct sackhole *tcp_sack_output(struct tcpcb *tp, int *sack_bytes_rexmt);
-void	 tcp_sack_partialack(struct tcpcb *, struct tcphdr *);
+void	 tcp_sack_partialack(struct tcpcb *, struct tcphdr *, uint32_t);
 void	 tcp_free_sackholes(struct tcpcb *tp);
 int	 tcp_newreno(struct tcpcb *, struct tcphdr *);
 u_long	 tcp_seq_subtract(u_long, u_long );
 
 void	cc_cong_signal(struct tcpcb *tp, struct tcphdr *th, uint32_t type);
 
+#define IS_MASTER_TP(tp) ((tp->t_mpcb->master_tcpcb) == (tp) ? 1 : 0)
+/*
+#define DIRECT_MAP(tp, var, op) do { \
+	(tp)->var op; \
+	if ((tp)->t_mpcb->direct_map) (tp)->t_mpcb->ds_##var = (tp)->var; \
+} while(0)
+*/
+
+struct mpcb *
+		mp_newmpcb(struct inpcb *);
+void 	mp_discardcb(struct mpcb *mp);
+void	mp_do_reass(struct tcpcb *tp);
+void	tcp_ackmap(struct tcpcb *tp, uint32_t acked, uint64_t data_ack_num);
+void	tcp_unmapped_init(void);
+void	tcp_unmapped_flush(struct tcpcb *tp);
+
 #endif /* _KERNEL */
 
 #endif /* _NETINET_TCP_VAR_H_ */
diff -r 1e55e1aa8b7f sys/sys/sockbuf.h
--- a/sys/sys/sockbuf.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/sys/sockbuf.h	Fri Jul 11 13:32:06 2014 +1000
@@ -53,6 +53,7 @@
 #define	SB_IN_TOE	0x400		/* socket buffer is in the middle of an operation */
 #define	SB_AUTOSIZE	0x800		/* automatically size socket buffer */
 #define	SB_STOP		0x1000		/* backpressure indicator */
+#define SB_DEFERACCT    0x2000		/* need to account for sb_cc and sb_actual separately */
 
 #define	SBS_CANTSENDMORE	0x0010	/* can't send more data to peer */
 #define	SBS_CANTRCVMORE		0x0020	/* can't receive more data from peer */
@@ -90,7 +91,8 @@
 					 * record in socket buffer */
 	struct	mbuf *sb_sndptr; /* (c/d) pointer into mbuf chain */
 	u_int	sb_sndptroff;	/* (c/d) byte offset of ptr into chain */
-	u_int	sb_cc;		/* (c/d) actual chars in buffer */
+	u_int	sb_cc;		/* (c/d) actively mapped bytes b/w application and protocol  */
+	u_int	sb_actual;	/* (c/d) actual chars in buffer */
 	u_int	sb_hiwat;	/* (c/d) max actual char count */
 	u_int	sb_mbcnt;	/* (c/d) chars of mbufs used */
 	u_int   sb_mcnt;        /* (c/d) number of mbufs in buffer */
@@ -163,12 +165,19 @@
 int	sblock(struct sockbuf *sb, int flags);
 void	sbunlock(struct sockbuf *sb);
 
+/* function to return sb_cc, i.e. chars in sockbuf */
+unsigned int sbgetcc(struct sockbuf *sb);
+
+
 /*
  * How much space is there in a socket buffer (so->so_snd or so->so_rcv)?
  * This is problematical if the fields are unsigned, as the space might
  * still be negative (cc > hiwat or mbcnt > mbmax).  Should detect
  * overflow and return 0.  Should use "lmin" but it doesn't exist now.
  */
+//#define	sbspace(sb) \
+//    ((long) lmin((u_int)((sb)->sb_hiwat - (sb)->sb_cc), \
+//	 (u_int)((sb)->sb_mbmax - (sb)->sb_mbcnt)))
 static __inline
 long
 sbspace(struct sockbuf *sb)
@@ -185,7 +194,10 @@
 
 /* adjust counters in sb reflecting allocation of m */
 #define	sballoc(sb, m) { \
-	(sb)->sb_cc += (m)->m_len; \
+	(sb)->sb_actual += (m)->m_len; \
+	atomic_add_acq_int(&(sb)->sb_cc,(m)->m_len); \
+	if ((sb)->sb_flags & SB_DEFERACCT) KASSERT((sb)->sb_cc <= (sb)->sb_actual, ("sb_cc greater than actual\n")); \
+	else KASSERT((sb)->sb_cc == (sb)->sb_actual, ("SB_DEFER sb_cc greater != actual\n")); \
 	if ((m)->m_type != MT_DATA && (m)->m_type != MT_OOBDATA) \
 		(sb)->sb_ctl += (m)->m_len; \
 	(sb)->sb_mbcnt += MSIZE; \
@@ -198,7 +210,10 @@
 
 /* adjust counters in sb reflecting freeing of m */
 #define	sbfree(sb, m) { \
-	(sb)->sb_cc -= (m)->m_len; \
+	if((sb)->sb_flags & SB_DEFERACCT) KASSERT((int)(sb)->sb_actual - (m)->m_len >= 0 && (sb)->sb_cc <= (sb)->sb_actual - (m)->m_len, ("SB_DEFERACCT sb_cc/sb_actual accounting error: sb_cc %u, sb_actual %u, len %d", (sb)->sb_cc, (sb)->sb_actual, (m)->m_len)); \
+	else KASSERT((int)(sb)->sb_cc - (m)->m_len >= 0 && (int)(sb)->sb_actual - (m)->m_len >= 0 && (sb)->sb_cc - (m)->m_len <= (sb)->sb_actual - (m)->m_len && (sb)->sb_cc == (sb)->sb_actual, ("sb_cc/sb_actual accounting error: sb_cc %u, sb_actual %u, len %d", (sb)->sb_cc, (sb)->sb_actual, (m)->m_len)); \
+	(sb)->sb_actual -= (m)->m_len; \
+	if (!((sb)->sb_flags & SB_DEFERACCT)) (sb)->sb_cc -= (m)->m_len; \
 	if ((m)->m_type != MT_DATA && (m)->m_type != MT_OOBDATA) \
 		(sb)->sb_ctl -= (m)->m_len; \
 	(sb)->sb_mbcnt -= MSIZE; \
diff -r 1e55e1aa8b7f sys/sys/socket.h
--- a/sys/sys/socket.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/sys/socket.h	Fri Jul 11 13:32:06 2014 +1000
@@ -42,6 +42,8 @@
  * Definitions related to sockets: types, address families, options.
  */
 
+extern int in_def_drop;
+
 /*
  * Data types.
  */
diff -r 1e55e1aa8b7f sys/sys/socketvar.h
--- a/sys/sys/socketvar.h	Sun May 04 01:33:02 2014 +0000
+++ b/sys/sys/socketvar.h	Fri Jul 11 13:32:06 2014 +1000
@@ -342,11 +342,16 @@
 	    struct thread *td);
 int	sopoll_generic(struct socket *so, int events,
 	    struct ucred *active_cred, struct thread *td);
+int	sopoll_mpstream(struct socket *so, int events,
+	    struct ucred *active_cred, struct thread *td);
 int	soreceive(struct socket *so, struct sockaddr **paddr, struct uio *uio,
 	    struct mbuf **mp0, struct mbuf **controlp, int *flagsp);
 int	soreceive_stream(struct socket *so, struct sockaddr **paddr,
 	    struct uio *uio, struct mbuf **mp0, struct mbuf **controlp,
 	    int *flagsp);
+int	soreceive_mpstream(struct socket *so, struct sockaddr **paddr,
+	    struct uio *uio, struct mbuf **mp0, struct mbuf **controlp,
+	    int *flagsp);
 int	soreceive_dgram(struct socket *so, struct sockaddr **paddr,
 	    struct uio *uio, struct mbuf **mp0, struct mbuf **controlp,
 	    int *flagsp);
