diff -r 11a8f1275a38 sys/conf/files
--- a/sys/conf/files	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/conf/files	Mon Mar 11 02:59:51 2013 +1100
@@ -3027,6 +3027,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 11a8f1275a38 sys/kern/uipc_sockbuf.c
--- a/sys/kern/uipc_sockbuf.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/kern/uipc_sockbuf.c	Mon Mar 11 02:59:51 2013 +1100
@@ -567,8 +567,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");
 	}
@@ -776,7 +776,14 @@
 			bcopy(mtod(m, caddr_t), mtod(n, caddr_t) + n->m_len,
 			    (unsigned)m->m_len);
 			n->m_len += m->m_len;
+			sb->sb_actual += m->m_len;
 			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)
 				/* XXX: Probably don't need.*/
 				sb->sb_ctl += m->m_len;
@@ -813,19 +820,26 @@
 		 * Don't call sbdrop(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;
-		sbdrop_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;
+		}
+		sbdrop_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);
 }
@@ -833,7 +847,6 @@
 void
 sbflush(struct sockbuf *sb)
 {
-
 	SOCKBUF_LOCK(sb);
 	sbflush_locked(sb);
 	SOCKBUF_UNLOCK(sb);
@@ -852,7 +865,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;
@@ -860,7 +873,18 @@
 		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)
@@ -908,7 +932,6 @@
 void
 sbdrop(struct sockbuf *sb, int len)
 {
-
 	SOCKBUF_LOCK(sb);
 	sbdrop_locked(sb, len);
 	SOCKBUF_UNLOCK(sb);
@@ -924,8 +947,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.
@@ -939,6 +962,11 @@
 	/* Return closest mbuf in chain for current offset. */
 	*moff = off - sb->sb_sndptroff;
 	m = ret = sb->sb_sndptr ? sb->sb_sndptr : sb->sb_mb;
+	if (*moff == m->m_len) {
+		*moff = 0;
+		sb->sb_sndptroff += m->m_len;
+		m = ret = m->m_next;
+	}
 
 	/* Advance by len to be as close as possible for the next transmit. */
 	for (off = off - sb->sb_sndptroff + len - 1;
diff -r 11a8f1275a38 sys/kern/uipc_socket.c
--- a/sys/kern/uipc_socket.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/kern/uipc_socket.c	Mon Mar 11 02:59:51 2013 +1100
@@ -111,6 +111,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>
@@ -125,6 +126,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>
@@ -138,6 +140,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>
@@ -185,6 +192,9 @@
 	VNET_ASSERT(curvnet != NULL,					\
 	    ("%s:%d curvnet is NULL, so=%p", __func__, __LINE__, (so)));
 
+/* MULTIPATH: for deferred reassembly */
+#define	sotoinpcb(so)	((struct inpcb *)(so)->so_pcb)
+
 static int somaxconn = SOMAXCONN;
 static int sysctl_somaxconn(SYSCTL_HANDLER_ARGS);
 /* XXX: we dont have SYSCTL_USHORT */
@@ -206,6 +216,7 @@
     &so_zero_copy_send, 0, "Enable zero copy send");
 #endif /* ZERO_COPY_SOCKETS */
 
+
 /*
  * accept_mtx locks down per-socket fields relating to accept queues.  See
  * socketvar.h for an annotation of the protected fields of struct socket.
@@ -1810,6 +1821,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);
@@ -2022,6 +2034,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__));
@@ -2112,6 +2125,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_DONTWAIT);
+			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()
@@ -3016,6 +3248,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;
@@ -3416,7 +3694,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 11a8f1275a38 sys/netinet/in_pcb.c
--- a/sys/netinet/in_pcb.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/in_pcb.c	Mon Mar 11 02:59:51 2013 +1100
@@ -321,6 +321,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)
@@ -1007,6 +1070,7 @@
 		if (error)
 			return (error);
 	}
+
 	oinp = in_pcblookup_hash_locked(inp->inp_pcbinfo, faddr, fport,
 	    laddr, lport, 0, NULL);
 	if (oinp != NULL) {
diff -r 11a8f1275a38 sys/netinet/in_pcb.h
--- a/sys/netinet/in_pcb.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/in_pcb.h	Mon Mar 11 02:59:51 2013 +1100
@@ -616,6 +616,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 11a8f1275a38 sys/netinet/mptcp.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp.h	Mon Mar 11 02:59:51 2013 +1100
@@ -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 11a8f1275a38 sys/netinet/mptcp_subr.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_subr.c	Mon Mar 11 02:59:51 2013 +1100
@@ -0,0 +1,1024 @@
+/*-
+ * 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>
+
+#include <netinet/in.h>
+#include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_fsm.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,
+    "Enable hacks for linux mptcp compatability");
+
+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, tcp_do_autosndbuf) = 1;
+#define	V_tcp_do_autosndbuf	VNET(tcp_do_autosndbuf)
+SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_auto, CTLFLAG_RW,
+	&VNET_NAME(tcp_do_autosndbuf), 0,
+	"Enable automatic send buffer sizing");
+
+VNET_DEFINE(int, tcp_autosndbuf_inc) = 8*1024;
+#define	V_tcp_autosndbuf_inc	VNET(tcp_autosndbuf_inc)
+SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_inc, CTLFLAG_RW,
+	&VNET_NAME(tcp_autosndbuf_inc), 0,
+	"Incrementor step size of automatic send buffer");
+
+VNET_DEFINE(int, tcp_autosndbuf_max) = 2*1024*1024;
+#define	V_tcp_autosndbuf_max	VNET(tcp_autosndbuf_max)
+SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_max, CTLFLAG_RW,
+	&VNET_NAME(tcp_autosndbuf_max), 0,
+	"Max size of automatic send buffer");
+
+struct debug_class {
+	char class[32];
+	int verbosity;
+};
+
+uint32_t mp_active_debug_classes;
+
+#define	N_DEBUGCLASSES 3
+struct debug_class debug_classes[N_DEBUGCLASSES] = {
+		{.class = "MPSESSION"},
+		{.class = "DSMAP"},
+		{.class = "SBSTATUS"}
+};
+
+//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 */
+int mp_address_count = 0;
+struct sockaddr_storage mp_usable_addresses[MAX_ADDRS];
+
+/* Initialise extern variables that store addresses */
+struct sockaddr_storage mp_path_manager[MAX_ADDRS];
+
+#define MSBIT_CHECK 0x80000000
+
+
+void
+mp_debug(uint32_t log_class, int msg_verbosity, uint32_t flags, char * fmt, ...) {
+	va_list ap;
+	int effective_verbosity, i;
+
+	effective_verbosity = 0;
+
+	if (log_class & mp_active_debug_classes) {
+	// calculate shift index (index of class into array)
+		i = 0;
+		while(log_class >= 1) {
+			if (log_class & 0x1)
+				effective_verbosity = max(debug_classes[i].verbosity, effective_verbosity);
+			log_class >>= 1;
+			i++;
+		}
+
+		if	(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);
+
+		for (i = 0; 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 = 0;
+		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) {
+				error = EINVAL;
+				mp_address_count = 0; //XXX: We shouldn't be reading into mp_usable_addresses directly...
+			}
+			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:%d", debug_classes[i].class, 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) {
+					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
+ */
+uint32_t
+mp_get_hmac(uint8_t *digest, struct mpcb *mp, struct tcpcb *tp) {
+	/*
+	* Set up the hmac key
+	*/
+	uint32_t keylen = sizeof(mp->local_key);
+	uint8_t *key_1_ptr;
+	uint8_t *key_2_ptr;
+	sctp_key_t *key_1;
+	sctp_key_t *key_2;
+
+	key_1 = sctp_alloc_key(keylen);
+	key_1->keylen = keylen;
+	key_1_ptr = key_1->key;
+	bcopy(&mp->local_key, key_1_ptr, keylen);
+
+	key_2 = sctp_alloc_key(keylen);
+	key_2->keylen = keylen;
+	key_2_ptr = key_2->key;
+	bcopy(&mp->remote_key, key_1_ptr, keylen);
+
+	sctp_key_t *hmac_key;
+	hmac_key = sctp_compute_hashkey(key_1, key_2, NULL);
+
+	/*
+	* Set up the hmac msg
+	*/
+	uint64_t hmac_msg;
+	hmac_msg = ((uint64_t)tp->local_rand << 32);
+	hmac_msg |= tp->remote_rand;
+
+	uint32_t hmac_len = sctp_compute_hmac(0x0001, 					//SCTP_HMAC
+			hmac_key, (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, "%x", *(bytes + bytes_len - i - 1));
+		else
+			sbuf_printf(&out, "%x", *(bytes + i));
+		i--;
+	}
+
+	sbuf_finish(&out);
+}
+
+void
+mp_get_recwin(unsigned long *recwin) {
+	return;
+}
+
+int
+mp_close_subflows() {
+	//XXXNJW: tdb
+	return (0);
+}
+
+int
+mp_close_subflow(struct tcpcb *tp) {
+	//XXXNJW: tdb
+	return (0);
+}
+
+
+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) {
+			tcp_output(sf_tp);
+			INP_WUNLOCK(sf_tp->t_inpcb);
+			break;
+		}
+		INP_WUNLOCK(sf_tp->t_inpcb);
+	}
+}
+
+/*
+ * 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;
+	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_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_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;
+
+		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, 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;
+		}
+
+		mp->mp_advjoin_mask &= ~(1 << i);
+
+		/* 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);
+
+		/* destination address from master_inpcb */
+		if(mp->master_inpcb->inp_vflag & INP_IPV4) {
+			bcopy(&mp->master_inpcb->inp_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) {
+			bcopy(&mp->master_inpcb->in6p_faddr,
+				&(((struct sockaddr_in6 *) &dst_nam)->sin6_addr.__u6_addr),
+				sizeof(struct in6_addr));
+			((struct sockaddr_in6 *) &dst_nam)->sin6_port
+				= mp->master_inpcb->inp_fport;
+			dst_nam.sa_len = sizeof(struct sockaddr_in6);
+			dst_nam.sa_family = AF_INET6;
+		}
+
+		/* ret > 1 if some kind of error */
+		if (ret) {
+			goto free;
+		}
+
+		tp = intotcpcb(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 from %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));
+
+
+		/* 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);
+
+	}
+
+	goto out;
+
+free:
+	in_pcbfree(sf_inp);
+out:
+	return;
+}
+
+
+void
+mp_data_ack(struct mpcb *mp, uint64_t data_ack_num) {
+
+	mtx_lock(&mp->mpcb_mutex);
+
+	if (data_ack_num > mp->ds_snd_una) {
+		atomic_subtract_int(&mp->master_socket->so_snd.sb_cc,
+		    data_ack_num - mp->ds_snd_una);
+		mp->ds_snd_una = data_ack_num;
+		mp_deferred_sbdrop(mp);
+	} 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
+	}
+
+	mtx_unlock(&mp->mpcb_mutex);
+}
+
+void
+mp_init_established(struct tcpcb *tp) {
+
+	/* Enabled multipath features */
+	tp->mp_active = 1;
+
+	/* we know the other end is mp enabled */
+	if (IS_MASTER_TP(tp)) {
+		tp->t_mpcb->mp_state = MPS_ESTABLISHED;
+		tp->t_mpcb->direct_map = 0;
+		tp->t_mpcb->ds_map_max = tp->t_mpcb->ds_snd_una =
+		    tp->t_mpcb->ds_map_min = tp->t_mpcb->ds_idsn;
+		tp->t_mpcb->ds_rcv_nxt = tp->t_mpcb->ds_idsr;
+
+		mp_debug(MPSESSION, 4, 0, "%s: idsn - %ju\n",
+		    __func__, (uintmax_t) tp->t_mpcb->ds_idsn);
+
+		mp_debug(MPSESSION, 4, 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, 4, 0, "%s: local key: %ju, remote key: %ju\n", __func__,
+			(uintmax_t) tp->t_mpcb->local_key,
+			(uintmax_t) tp->t_mpcb->remote_key);
+	} else {
+		mp_debug(MPSESSION, 4, 0, "%s: established slave subflow\n",__func__);
+	}
+
+
+}
+
+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;
+
+	cannotfreembptr = NULL;
+
+	/* Keep a list of maps that should be freed only if ndrop is > 0 */
+	struct dsmapq_head maps_to_free;
+	TAILQ_INIT(&maps_to_free);
+
+	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, 4, 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);
+
+		/*
+		 * Can we drop this map?
+		 * - Has it been cumulatively ACKED
+		 * - Does the sequence space start and end before ds_snd_una
+		 * - Does the maps data region fall within a single mbuf
+		 *
+		 */
+		if ((map->flags & MAPF_IS_ACKED) && map->ds_map_start < mp->ds_snd_una
+		    && ds_map_end <= mp->ds_snd_una) {
+			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, 4, 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, 4, 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 {
+			mp_debug(DSMAP, 4, 0, "%s: map overlap below una, can't drop anything\n", __func__);
+			/* We have overlap between a completed and uncompleted map this
+			 * is below una or we have a map that overlaps and extends past
+			 * una */
+
+				ndrop = 0;
+				break;
+		}
+	}
+
+	/* Error as we are dropping more than in the sb */
+	if (ndrop > mp->master_socket->so_snd.sb_actual) {
+			panic("%s: ndrop greater than chars in sendbuffer\n", __func__);
+	}
+
+	if (ndrop > 0) {
+		int bytes_from_end;
+		struct mbuf *map_mbuf_end;
+
+		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;
+			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, 4, 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);
+
+				mp_debug(DSMAP, 4, 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;
+	     }
+
+
+	     mp_debug(DSMAP, 4, 0, "%s: Dropping %d bytes from socket "
+	         "buffer\n", __func__, ndrop);
+	     sbdrop(&mp->master_socket->so_snd, ndrop);
+	     mp->ds_map_min += ndrop;
+	     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 just returns the size of the send buffer (i.e. no
+ * 'smarts' in what it allocates to a subflow)
+ */
+struct ds_map*
+mp_get_map(struct mpcb *mp) {
+	struct ds_map *map = NULL;
+	struct socket *so;
+	int map_length;
+
+	so = mp->master_socket;
+	SOCKBUF_LOCK(&so->so_snd);
+
+	/* If there are bytes in the send buff that could potentially be
+	 * sent
+	 * */
+	if(so->so_snd.sb_cc) {
+
+		//if (so->so_snd.sb_cc >= so->so_snd.sb_hiwat)
+		//	map_length = SB_NUNMAPPED(&so->so_snd, mp) / mp->subflow_cnt;
+		//else
+		//	map_length = SB_NUNMAPPED(&so->so_snd, mp);
+
+		/* the following line forces a max map_length of 2048
+		 * which is used in testing to spread the socket buffer
+		 * across more flows */
+		map_length = min(SB_NUNMAPPED(&so->so_snd, mp), 2048);
+
+		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;
+
+				// 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) {
+						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: DEBUG */
+				int total_mapped = 0;
+				TAILQ_FOREACH(next_map, &mp->mp_ds_sndmaps, mp_ds_map_next) {
+					total_mapped += next_map->ds_map_len;
+				}
+
+				KASSERT(total_mapped == mp->ds_map_max - mp->ds_map_min,
+						("total mapped not equal to mapped bytesl\n"));
+
+			}
+		}
+	}
+
+	SOCKBUF_UNLOCK(&so->so_snd);
+
+	return(map);
+}
+
+struct ds_map *
+mp_find_dsmap(struct tcpcb *tp, tcp_seq	seqnum)
+{
+	struct ds_map *map;
+	int new_map;
+
+	new_map = 0;
+
+	TAILQ_FOREACH(map, &tp->t_txdsmaps, sf_ds_map_next) {
+		if (seqnum >= map->sf_seq_start) {
+			if (seqnum >= (map->sf_seq_start + map->ds_map_len)) {
+				/* seqnum should be equal to tp->snd_max but snd_max may not have
+				 * been updated before this function has been called. */
+				KASSERT(seqnum == (map->sf_seq_start + map->ds_map_len) &&
+					map->flags & MAPF_IS_SENT,
+				    ("%s: sqn %u is greater than highest map\n", __func__,
+				    seqnum));
+				new_map = 1;
+			}
+			break;
+		}
+	}
+
+	if (map == NULL || new_map) {
+		if (map == NULL)
+			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);
+		if (map) {
+			map->sf_seq_start = seqnum;
+			TAILQ_INSERT_HEAD(&tp->t_txdsmaps, map, sf_ds_map_next);
+			if (tp->sf_flags & SFF_DATA_WAIT) {
+				tp->sf_flags &= ~SFF_DATA_WAIT;
+				atomic_subtract_acq_int(&tp->t_mpcb->data_wait_count, 1);
+			}
+			//if (atomic_load_acq_int(&tp->t_mpcb->data_wait_count) > 1)
+			//	taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->data_task);
+		} else {
+			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);
+}
+
+void
+mp_subflow_is_disconnected(struct tcpcb *tp) {
+
+	struct mpcb *mp = tp->t_mpcb;
+	struct tcpcb *sf_tp;
+	int all_closed = 1;
+
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+
+	SLIST_FOREACH(sf_tp, &mp->tp_list, next_subflow) {
+		if (sf_tp == tp)
+			continue;
+
+		/*
+		 * 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);
+		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)
+		soisdisconnected(mp->master_socket);
+
+}
+
+void
+mp_process_local_key(struct mpcb *mp, uint64_t local_key)
+{
+	uint8_t digest[20];
+	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);
+
+}
+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->ds_idsr = htobe64(mp->ds_idsr);
+	mp->remote_token = mp_get_token(digest);
+	mp->remote_key = remote_key;
+
+	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(key) = 0x%s\n", buf);
+
+	btohex(buf, sizeof(buf), (uint8_t *)&mp->ds_map_max,
+			sizeof(mp->ds_map_max), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "ds_map_max = 0x%s\n", buf);
+	mp_debug(MPSESSION, 4, 0, "ds_map_max = %u\n", (uint32_t)mp->ds_map_max);
+
+	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);
+
+	btohex(buf, sizeof(buf), (uint8_t *)&mp->ds_rcv_nxt,
+			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_rcv_nxt);
+}
+
+
+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 11a8f1275a38 sys/netinet/mptcp_var.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_var.h	Mon Mar 11 02:59:51 2013 +1100
@@ -0,0 +1,416 @@
+/*-
+ * 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 */
+
+	/*
+	 * 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 */
+
+	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 */
+
+	int subflow_cnt;		/* number of subflows created */
+	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
+
+	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;	/* Sending window for sub-flows */
+
+	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;	/* max rcv_wnd of all subflows */
+
+	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 */
+
+	/*
+	 * 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 */
+};
+
+#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 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 SB_NUNMAPPED(sb, mp) ((sb)->sb_actual - ((mp)->ds_map_max - (mp)->ds_map_min))
+
+/*
+ * 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(mpcb)	rw_init(&mp->reass_queues_lock, "reass queues lock")
+#define	SF_REASS_LOCK_DESTROY(mpcb)	rw_destroy(&mp->reass_queues_lock)
+#define	SF_REASS_WLOCK(mpcb)	rw_wlock(&mp->reass_queues_lock)
+#define	SF_REASS_WUNLOCK(mpcb)	rw_wunlock(&mp->reass_queues_lock)
+#define	SF_REASS_RLOCK(mpcb)	rw_rlock(&mp->reass_queues_lock)
+#define	SF_REASS_RUNLOCK(mpcb)	rw_runlock(&mp->reass_queues_lock)
+
+
+/* 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
+
+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, struct mpcb *mp, struct tcpcb *tp);
+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);
+
+/* 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);
+int		mp_accept_subflow_connection(struct inpcb *inp, struct sockaddr *nam, struct thread *td);
+int		tcp_do_subflow_listen(struct inpcb *inp, struct thread *td);
+
+/* tcp_output related */
+struct ds_map*
+		mp_get_map(struct mpcb *mp);
+void	mp_get_recwin(unsigned long *recwin);
+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_subflow_is_disconnected(struct tcpcb *tp);
+
+/* Function for task queue */
+void	mp_join_task_handler(void *context, int pending);
+void	mp_datascheduler_task_handler(void *context, int pending);
+
+
+
+#endif /* MPTCP_VAR_H_ */
diff -r 11a8f1275a38 sys/netinet/tcp.h
--- a/sys/netinet/tcp.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp.h	Mon Mar 11 02:59:51 2013 +1100
@@ -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 11a8f1275a38 sys/netinet/tcp_input.c
--- a/sys/netinet/tcp_input.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_input.c	Mon Mar 11 02:59:51 2013 +1100
@@ -56,6 +56,7 @@
 #include "opt_ipsec.h"
 #include "opt_tcpdebug.h"
 
+#include <sys/endian.h>
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/hhook.h>
@@ -63,12 +64,18 @@
 #include <sys/mbuf.h>
 #include <sys/proc.h>		/* for proc0 declaration */
 #include <sys/protosw.h>
+#include <sys/sbuf.h>
 #include <sys/signalvar.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
 #include <sys/syslog.h>
 #include <sys/systm.h>
+#include <sys/kdb.h>
+
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/limits.h>
 
 #include <machine/cpu.h>	/* before tcp_seq.h, for tcp_random18() */
 
@@ -109,6 +116,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>
@@ -136,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");
@@ -209,6 +222,14 @@
     &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");
+
 VNET_DEFINE(struct inpcbhead, tcb);
 #define	tcb6	tcb  /* for KAME src sync over BSD*'s */
 VNET_DEFINE(struct inpcbinfo, tcbinfo);
@@ -237,6 +258,16 @@
 			    struct tcphdr *th, struct tcpopt *to);
 
 /*
+ * 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);
+
+#define MSBIT_CHECK 0x80000000
+
+/*
  * Kernel module interface for updating tcpstat.  The argument is an index
  * into tcpstat treated as an array of u_long.  While this encodes the
  * general layout of tcpstat into the caller, it doesn't encode its location,
@@ -250,6 +281,288 @@
 	(*((u_long *)&V_tcpstat + statnum))++;
 }
 
+
+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:
+	{
+		// 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;
+
+			//dss_flags = (uint8_t) cp + MP_DSS_FLAGS_OFFSET;
+			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;
+					bcopy((char *)cp + MP_DATA_ACK64_OFFSET,	// grabbing whole 64-bits
+						(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);
+					byte_offset += 8;	// offset the presence of 64-bit ACK
+				} 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 = (uint64_t) ntohl(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);
+
+				/* checksum */
+				// XXX: 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....
+
+//					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:
+	{
+		/* SYN-only segments are processed in mp_check_for_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));
+			to->to_mopts.rcv_token =
+					ntohl(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.snd_rnd =
+								ntohl(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));
+			to->to_mopts.snd_trc_mac =
+			    be64toh(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.snd_rnd =
+			    ntohl(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;
+			/* sha is byte stream so byte order should be okay... */
+			bcopy((char *)cp + MP_SND_MAC_OFFSET,
+					(char *)to->to_mopts.snd_mac,
+					sizeof(to->to_mopts.snd_mac));
+
+			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);
+
+		/* 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.
  */
@@ -347,10 +660,11 @@
 	 * RFC3390 says only do this if SYN or SYN/ACK didn't got lost.
 	 * XXX: We currently check only in syncache_socket for that.
 	 */
-	if (V_tcp_do_rfc3390)
+	if (V_tcp_do_rfc3390) {
 		tp->snd_cwnd = min(4 * tp->t_maxseg,
 		    max(2 * tp->t_maxseg, 4380));
-	else
+		//max(2 * tp->t_maxseg, ((4380/tp->t_maxseg) * tp->t_maxseg)));
+	} else
 		tp->snd_cwnd = tp->t_maxseg;
 
 	if (CC_ALGO(tp)->conn_init != NULL)
@@ -838,10 +1152,12 @@
 		m_tag_delete(m, fwd_tag);
 	} else
 #endif /* IPFIREWALL_FORWARD */
+
 		inp = in_pcblookup_mbuf(&V_tcbinfo, ip->ip_src,
 		    th->th_sport, ip->ip_dst, th->th_dport,
 		    INPLOOKUP_WILDCARD | INPLOOKUP_WLOCKPCB,
 		    m->m_pkthdr.rcvif, m);
+
 #endif /* INET */
 
 	/*
@@ -1027,6 +1343,7 @@
 	 */
 	if (so->so_options & SO_ACCEPTCONN) {
 		struct in_conninfo inc;
+		int is_subflow = 0;
 
 		KASSERT(tp->t_state == TCPS_LISTEN, ("%s: so accepting but "
 		    "tp not listening", __func__));
@@ -1054,17 +1371,52 @@
 		 * 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
+			 *
+			 */
+
+//			printf("%s: ACK -> found inp at %p\n", __func__, inp);
+
 			/*
 			 * NB: syncache_expand() doesn't unlock
 			 * inp and tcpinfo locks.
 			 */
-			if (!syncache_expand(&inc, &to, th, &so, m)) {
+//			if (!syncache_expand(&inc, &to, th, &so, m)) {
+//				/*
+//				 * No syncache entry or ACK was not
+//				 * for our SYN/ACK.  Send a RST.
+//				 * NB: syncache did its own logging
+//				 * of the failure cause.
+//				 */
+//				rstreason = BANDLIM_RST_OPENPORT;
+//				goto dropwithreset;
+//			}
+
+			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.
@@ -1074,6 +1426,7 @@
 				rstreason = BANDLIM_RST_OPENPORT;
 				goto dropwithreset;
 			}
+
 			if (so == NULL) {
 				/*
 				 * We completed the 3-way handshake
@@ -1098,17 +1451,21 @@
 				} 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));
+			}
+
+			INP_WLOCK(tp->t_inpcb); /* new connection */
 			KASSERT(tp->t_state == TCPS_SYN_RECEIVED,
 			    ("%s: ", __func__));
+
 #ifdef TCP_SIGNATURE
 			if (sig_checked == 0)  {
 				tcp_dooptions(&to, optp, optlen,
@@ -1330,7 +1687,9 @@
 			tcp_trace(TA_INPUT, ostate, tp,
 			    (void *)tcp_saveipgen, &tcp_savetcp, 0);
 #endif
-		tcp_dooptions(&to, optp, optlen, TO_SYN);
+
+		tcp_dooptions(&to, optp, optlen, TO_SYN);																	// HERE
+
 		syncache_add(&inc, &to, th, inp, &so, m, NULL, NULL);
 		/*
 		 * Entry added to syncache and mbuf consumed.
@@ -1419,10 +1778,13 @@
     int ti_locked)
 {
 	int thflags, acked, ourfinisacked, needoutput = 0;
-	int rstreason, todrop, win;
+	int rstreason = 0, todrop, win;
 	u_long tiwin;
 	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,
@@ -1529,12 +1891,293 @@
 			to.to_tsecr = 0;
 	}
 
+//	if (tp->t_mpcb->mp_state & MPS_PRE_ESTABLISHED)
+//		mp_init_established(tp);
+
+	/*
+	 * Need to send a data-level ACK in response to this segment
+	 */
+	if (to.to_mopts.mpo_flags & MPOF_NEED_ACK) {
+
+		//XXXNJW done with upcall to mp layer OR pass this info into tcp_output
+		// (via tcptops) and tcp_output can generate the D-ACK
+		tp->t_mpcb->mp_flags |= MPO_DATA_ACK;
+		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, 4, 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;
+		}
+		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);
+		}
+	}
+
+	/*
+	 * Got a new DSS mapping for this subflow.
+	 */
+	if ((to.to_mopts.mpo_flags & MPOF_DSS)) {
+		//if(mp->mp_state == MPS_PRE_ESTABLISHED )
+			//mp->mp_state = MPS_ESTABLISHED;
+
+		/*
+		 * 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 */
+			if(mp->direct_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) {
+					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)) {
+			if(mp->direct_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) {
+					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);
+				}
+			}
+		}
+
+		/*
+		 * Got a data-level ACK
+		 */
+		if ((to.to_mopts.mpo_flags & MPOF_DATA_ACK)) {
+			to.to_mopts.mpo_flags &= ~MPOF_DATA_ACK;
+			mp_data_ack(mp, to.to_mopts.data_ack_num);
+		}
+
+		/*
+		 * XXX: inifnite 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)
+			printf("%s: processed DFIN option\n",__func__);
+			//tp->t_state = TCPS_CLOSE_WAIT;						//XXXNJW to uncomment
+
+
+		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");
+
+			/* find the map that this segment belongs to */
+			TAILQ_FOREACH(map, &tp->t_rxdsmaps, sf_ds_map_next) {
+				if (to.to_mopts.data_seq_num
+				    > map->ds_map_start + map->ds_map_len) {
+					map = NULL;
+					break;
+				}
+
+				if (to.to_mopts.data_seq_num >= map->ds_map_start)
+					break;
+			}
+
+			/*
+			 * Could not find a map covered by this sequence number,
+			 * so we allocate a new map and put into mapping list.
+			 *
+			 * Also for cases where !has_dsn_map (map will be NULL
+			 * in this case)
+			 *
+			 * XXX: should use zone allocator
+			 */
+
+			/*
+			 * XXXNJW: Need to think about coalescing mappings that overlap on
+			 * the right hand edge.
+			 * X
+			 */
+			if (map == NULL) {
+				int found_dup_map = 0;
+
+				/* inserting receive maps in subflow sequence order */
+				struct ds_map *next_map;
+				TAILQ_FOREACH(next_map, &tp->t_rxdsmaps, sf_ds_map_next) {
+					if (to.to_mopts.sub_seq_num + tp->irs > next_map->sf_seq_start) {
+						break;
+					} else if (to.to_mopts.sub_seq_num + tp->irs ==
+					      next_map->sf_seq_start) {
+						KASSERT(to.to_mopts.data_seq_num == next_map->ds_map_start
+								&& to.to_mopts.dss_data_len == next_map->ds_map_len
+								&& to.to_mopts.dss_csum == next_map->ds_map_csum
+								,("SF sequence number matches existing map "
+								 "but maps are not the same\n"));
+						found_dup_map = 1;
+						break;
+					}
+				}
+
+				if (!found_dup_map) {
+
+					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 = to.to_mopts.dss_data_len;
+					map->ds_map_csum = to.to_mopts.dss_csum;
+
+					/* if there are no existing maps */
+					if (next_map == NULL) {
+						KASSERT(TAILQ_EMPTY(&tp->t_rxdsmaps),
+							("Incoming sequence less than those in rx map\n"));
+						TAILQ_INSERT_HEAD(&tp->t_rxdsmaps, map, sf_ds_map_next);
+					} else {
+						TAILQ_INSERT_BEFORE(next_map, map, sf_ds_map_next);
+					}
+
+//					printf("inserted map: ds_map_start %ju : %u, sf_seq_start: %u. len: %d\n",
+//						(uintmax_t) map->ds_map_start, (uint32_t) map->ds_map_start,
+//						map->sf_seq_start, map->ds_map_len);
+				} else {
+					printf("skipped duplicate map: ds_map_start %ju : %u, sf_seq_start: %u. len: %d\n",
+					    (uintmax_t) next_map->ds_map_start, (uint32_t) next_map->ds_map_start,
+					    next_map->sf_seq_start, next_map->ds_map_len);
+				}
+
+				/*
+				 * 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->t_mpcb->direct_map) {
+					tp->t_mpcb->ds_rcv_nxt = to.to_mopts.data_seq_num;
+					tp->t_mpcb->direct_map = 0;
+				}
+			}
+		}
+	}
+
+	/*
+	 * 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(&mp_usable_addresses[to.to_mopts.addr_id],
+		    &to.to_mopts.new_addr, sizeof(struct sockaddr_storage));
+	}
+
+
+	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.
+		 */
+		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->t_mpcb->direct_map) {
+			dtag->dsn = th->th_seq;
+		} else {
+			if (map == NULL) {
+				/* find the map that this segment belongs to */
+				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.
+					 */
+					if (th->th_seq > map->sf_seq_start + map->ds_map_len)
+						panic("%s: Received data beyond end of mapped region\n", __func__);
+
+					/* A map exists for this packet */
+					if (th->th_seq >= map->sf_seq_start)
+						break;
+				}
+
+//				printf("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);
+			}
+
+			KASSERT(map != NULL, ("%s:%d No map found for packet with "
+			    "length %d, mp_active is: %d\n",
+			    __func__, __LINE__, tlen, tp->mp_active));
+			/* update map details */
+			dtag->dsn = map->ds_map_start + th->th_seq - map->sf_seq_start;
+
+			if (th->th_seq + tlen == map->sf_seq_start + map->ds_map_len) {
+				TAILQ_REMOVE(&tp->t_rxdsmaps, map, sf_ds_map_next);
+				free(map, M_DSSMAP);
+			}
+		}
+
+		/* And prepend the mtag, to be checked in reass */
+		m_tag_prepend(m, mtag);
+	}
+
 	/*
 	 * 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.
+	 */
+
+	/*
+	 * There is some access to a shared structure in here, but at this
+	 * point there will only ever be a single subflow, so this might
+	 * not cause any problems?
 	 */
 	if (tp->t_state == TCPS_SYN_SENT && (thflags & TH_SYN)) {
 		if ((to.to_flags & TOF_SCALE) &&
@@ -1557,8 +2200,60 @@
 		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 */
+					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 an MP_CAPABLE but did not send this option */
+					tp->mpc = 1;
+					tp->mp_active = 0;
+				}
+			} else {
+				// could be a subflow SYN
+				if (to.to_mopts.mpo_flags & MPOF_JOIN_SYNACK) {
+
+					if (to.to_mopts.mpo_flags & MPOF_BACKUP_PATH) {
+						// this flow is a backup path
+					}
+
+					/* mp is available on this flow */
+					tp->mpc = 1;
+
+					// sender's truncated MAC = to.to_mopts.snd_trc_mac
+					tp->remote_rand = to.to_mopts.snd_rnd;
+
+					// need to calculate a MAC to return in the MP_JOIN ACK
+					// using the random number just rcvd, and the keys from
+					// the initial mp establishment
+
+					/* Calculate MAC-A */
+					uint8_t digest[20];
+					uint32_t mac_len = mp_get_hmac(digest, mp, tp);
+
+					if (mac_len == 0) {
+						printf("%s: error computing MAC-A\n", __func__);
+						goto dropwithreset;
+					}
+
+					tp->sf_flags |= SFF_GOT_JOIN_SYNACK;
+				}
+			}
+		}
+
 	}
 
+
 	/*
 	 * Header prediction: check for the two common cases
 	 * of a uni-directional data xfer.  If the packet has
@@ -1582,7 +2277,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)) ) {
 
@@ -1653,8 +2348,7 @@
 				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;
@@ -1666,8 +2360,17 @@
 				 * window.
 				 */
 				cc_ack_received(tp, th, CC_ACK);
-
 				tp->snd_una = th->th_ack;
+
+				/* 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->t_mpcb->direct_map) {
+					atomic_subtract_int(&so->so_snd.sb_cc, acked);
+				}
+				tcp_ackmap(tp, th);
+
 				/*
 				 * Pull snd_wl2 up to prevent seq wrap relative
 				 * to th_ack.
@@ -1697,8 +2400,11 @@
 				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;
 			}
@@ -1719,7 +2425,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.
@@ -1732,7 +2437,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,
@@ -1771,6 +2476,11 @@
 		 * TODO: Only step up if the application is actually serving
 		 * the buffer to better manage the socket buffer resources.
 		 */
+
+			/*
+			 * XXXNJW: Auto buffer tuning will 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)) {
@@ -1806,10 +2516,17 @@
 					    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);
+			//sorwakeup_locked(so); // this is called from tcp_reass now
+
 			if (DELAY_ACK(tp)) {
 				tp->t_flags |= TF_DELACK;
 			} else {
@@ -1879,6 +2596,8 @@
 			goto drop;
 
 		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);
@@ -1894,6 +2613,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.
@@ -1908,7 +2628,7 @@
 				tp->t_flags |= TF_ECN_PERMIT;
 				TCPSTAT_INC(tcps_ecn_shs);
 			}
-			
+
 			/*
 			 * Received <SYN,ACK> in SYN_SENT[*] state.
 			 * Transitions:
@@ -1925,6 +2645,17 @@
 				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
+						/* Don't want a new subflow to switch the mp state */
+						if(IS_MASTER_TP(tp))
+							mp_init_established(tp);
+					}
+				}
 			}
 		} else {
 			/*
@@ -2329,6 +3060,12 @@
 			tp->t_state = TCPS_ESTABLISHED;
 			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()
@@ -2349,6 +3086,12 @@
 	 * 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->sf_state = MPS_ESTABLISHED;
+			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:
@@ -2408,7 +3151,7 @@
 						
 						/*
 						 * 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.
 						 */
@@ -2419,13 +3162,13 @@
 							if (tp->snd_cwnd > tp->snd_ssthresh)
 								tp->snd_cwnd = tp->snd_ssthresh;
 						}
-					} else
+					} else {		// duplicate acks
 						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
@@ -2488,6 +3231,12 @@
 					    tp->t_maxseg;
 					(void) tcp_output(tp);
 					sent = tp->snd_max - oldsndmax;
+
+//					printf("limited transmit: oldcwnd: %lu, snd_cwnd: %lu, snd_wnd: %lu, dupacks %d, snd_limited %d, "
+//							"sent %u, snd_nxt: %u, snd_max: %u \n\n",
+//							oldcwnd, tp->snd_cwnd, tp->snd_wnd, tp->t_dupacks,
+//							tp->snd_limited, sent, tp->snd_nxt , tp->snd_max);
+
 					if (sent > tp->t_maxseg) {
 						KASSERT((tp->t_dupacks == 2 &&
 						    tp->snd_limited == 0) ||
@@ -2537,6 +3286,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)) {
@@ -2616,18 +3366,14 @@
 		 */
 		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;
-			sbdrop_locked(&so->so_snd, (int)so->so_snd.sb_cc);
+		if ((tp->t_flags & TF_SENTFIN) && (th->th_ack == tp->snd_max)) {
+			tp->snd_wnd -= ulmin(tp->snd_wnd, acked);
 			ourfinisacked = 1;
 		} else {
-			sbdrop_locked(&so->so_snd, acked);
 			tp->snd_wnd -= acked;
 			ourfinisacked = 0;
 		}
-		/* NB: sowwakeup_locked() does an implicit unlock. */
-		sowwakeup_locked(so);
+
 		/* Detect una wraparound. */
 		if (!IN_RECOVERY(tp->t_flags) &&
 		    SEQ_GT(tp->snd_una, tp->snd_recover) &&
@@ -2639,6 +3385,13 @@
 			EXIT_RECOVERY(tp->t_flags);
 		}
 		tp->snd_una = th->th_ack;
+
+		/* XXX this is a temp hack, must be changed */
+		if (tp->t_mpcb->direct_map) {
+			atomic_subtract_int(&so->so_snd.sb_cc, acked - (ourfinisacked ? 1 : 0));
+		}
+		tcp_ackmap(tp, th);
+
 		if (tp->t_flags & TF_SACK_PERMIT) {
 			if (SEQ_GT(tp->snd_una, tp->snd_recover))
 				tp->snd_recover = tp->snd_una;
@@ -2667,7 +3420,11 @@
 				 * compressed state.
 				 */
 				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 :
@@ -2712,6 +3469,10 @@
 step6:
 	INP_WLOCK_ASSERT(tp->t_inpcb);
 
+//	if((tp->t_mpcb->mp_state == MPS_PRE_ESTABLISHED) &&
+//			(thflags & (TH_SYN|TH_ACK)) == TH_ACK)
+//		mp_init_established(tp);
+
 	/*
 	 * Update window information.
 	 * Don't look at window if no ACK: TAC's send garbage on first SYN.
@@ -2744,7 +3505,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 */
@@ -2766,7 +3527,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;
@@ -2799,17 +3560,21 @@
 
 	/*
 	 * 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
 	 * connection then we just ignore the text.
 	 */
+
+
 	if ((tlen || (thflags & TH_FIN)) &&
 	    TCPS_HAVERCVDFIN(tp->t_state) == 0) {
 		tcp_seq save_start = th->th_seq;
 		m_adj(m, drop_hdrlen);	/* delayed header drop */
+
 		/*
+		 * 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
@@ -2821,35 +3586,21 @@
 		 * 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))
-				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))
+					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
@@ -3099,8 +3850,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)
@@ -3144,6 +3898,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
diff -r 11a8f1275a38 sys/netinet/tcp_output.c
--- a/sys/netinet/tcp_output.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_output.c	Mon Mar 11 02:59:51 2013 +1100
@@ -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>
@@ -49,6 +52,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>
@@ -78,7 +82,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*/
@@ -92,7 +97,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,
@@ -103,29 +108,32 @@
 SYSCTL_VNET_INT(_net_inet_tcp, TCPCTL_SENDSPACE, sendspace, CTLFLAG_RW,
 	&VNET_NAME(tcp_sendspace), 0, "Initial send socket buffer size");
 
-VNET_DEFINE(int, tcp_do_autosndbuf) = 1;
-#define	V_tcp_do_autosndbuf	VNET(tcp_do_autosndbuf)
-SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_auto, CTLFLAG_RW,
-	&VNET_NAME(tcp_do_autosndbuf), 0,
-	"Enable automatic send buffer sizing");
-
-VNET_DEFINE(int, tcp_autosndbuf_inc) = 8*1024;
-#define	V_tcp_autosndbuf_inc	VNET(tcp_autosndbuf_inc)
-SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_inc, CTLFLAG_RW,
-	&VNET_NAME(tcp_autosndbuf_inc), 0,
-	"Incrementor step size of automatic send buffer");
-
-VNET_DEFINE(int, tcp_autosndbuf_max) = 2*1024*1024;
-#define	V_tcp_autosndbuf_max	VNET(tcp_autosndbuf_max)
-SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_max, CTLFLAG_RW,
-	&VNET_NAME(tcp_autosndbuf_max), 0,
-	"Max size of automatic send buffer");
+//VNET_DEFINE(int, tcp_do_autosndbuf) = 1;
+//#define	V_tcp_do_autosndbuf	VNET(tcp_do_autosndbuf)
+//SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_auto, CTLFLAG_RW,
+//	&VNET_NAME(tcp_do_autosndbuf), 0,
+//	"Enable automatic send buffer sizing");
+//
+//VNET_DEFINE(int, tcp_autosndbuf_inc) = 8*1024;
+//#define	V_tcp_autosndbuf_inc	VNET(tcp_autosndbuf_inc)
+//SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_inc, CTLFLAG_RW,
+//	&VNET_NAME(tcp_autosndbuf_inc), 0,
+//	"Incrementor step size of automatic send buffer");
+//
+//VNET_DEFINE(int, tcp_autosndbuf_max) = 2*1024*1024;
+//#define	V_tcp_autosndbuf_max	VNET(tcp_autosndbuf_max)
+//SYSCTL_VNET_INT(_net_inet_tcp, OID_AUTO, sendbuf_max, CTLFLAG_RW,
+//	&VNET_NAME(tcp_autosndbuf_max), 0,
+//	"Max size of automatic send buffer");
 
 static void inline	hhook_run_tcp_est_out(struct tcpcb *tp,
 			    struct tcphdr *th, struct tcpopt *to,
 			    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 ouput helper hook.
  */
@@ -159,6 +167,372 @@
 		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 = mp_address_count;
+
+			subtype = (subtype << 4);
+
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = (MPTCP_SUBTYPE_MP_JOIN << 4);	// just manually setting
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = addrID;
+
+			to->to_mopts.rcv_token = htonl(to->to_mopts.rcv_token);
+			bcopy((u_char *)&to->to_mopts.rcv_token,
+					optp, sizeof(to->to_mopts.rcv_token));
+			optp += sizeof(to->to_mopts.rcv_token);
+
+			to->to_mopts.snd_rnd = htonl(to->to_mopts.snd_rnd);
+			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;
+
+			to->to_mopts.truncated_MAC = htobe64(to->to_mopts.truncated_MAC);
+			bcopy((u_char *)&to->to_mopts.truncated_MAC,
+			    optp, sizeof(to->to_mopts.truncated_MAC));
+			optp += sizeof(to->to_mopts.truncated_MAC);
+
+			to->to_mopts.snd_rnd = htonl(to->to_mopts.snd_rnd);
+			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;
+
+			uint32_t tempval = 0;
+			bcopy((u_char *)&tempval,
+					optp, sizeof(tempval));
+			optp += sizeof(tempval);
+
+			uint64_t tempval2 = 0;
+			bcopy((u_char *)&tempval2,
+					optp, sizeof(tempval2));
+			optp += sizeof(tempval2);
+
+			bcopy((u_char *)&tempval2,
+					optp, sizeof(tempval2));
+			optp += sizeof(tempval2);
+			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
+			// XXX 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_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 < mp_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  */
+				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 ++;
+
+				/* Address ID */
+				bcopy((u_char *)&i, 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.
  */
@@ -167,7 +541,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_unsent, error = 0;	/* Keep compiler happy */
+	struct ds_map *snd_dsmap;
 	struct mbuf *m;
 	struct ip *ip = NULL;
 	struct ipovly *ipov = NULL;
@@ -199,6 +574,7 @@
 		return (tcp_offload_output(tp));
 #endif
 
+
 	/*
 	 * Determine length of data that should be transmitted,
 	 * and flags that will be used.
@@ -227,10 +603,9 @@
 	sendalot = 0;
 	tso = 0;
 	mtu = 0;
-	off = tp->snd_nxt - tp->snd_una;
 	sendwin = min(tp->snd_wnd, tp->snd_cwnd);
+	flags = tcp_outflags[tp->t_state];
 
-	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.
@@ -238,6 +613,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.
 	 */
@@ -248,7 +624,7 @@
 	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;
 		if (cwin < 0)
 			cwin = 0;
@@ -267,6 +643,7 @@
 				 * moves past p->rxmit.
 				 */
 				p = NULL;
+				tp->snd_nxt = tp->snd_max;			//XXX experimenting
 				goto after_sack_rexmit;
 			} else
 				/* Can rexmit part of the current hole */
@@ -274,15 +651,19 @@
 						   tp->snd_recover - p->rxmit));
 		} else
 			len = ((long)ulmin(cwin, p->end - p->rxmit));
-		off = p->rxmit - tp->snd_una;
+		//off = p->rxmit - tp->snd_una;
+
 		KASSERT(off >= 0,("%s: sack block to the left of una : %d",
 		    __func__, off));
 		if (len > 0) {
+			tp->snd_nxt = p->rxmit;					//XXX 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:
@@ -295,7 +676,24 @@
 	if (tp->t_flags & TF_NEEDSYN)
 		flags |= TH_SYN;
 
-	SOCKBUF_LOCK(&so->so_snd);
+	off = tp->snd_nxt - tp->snd_una;
+
+	if((flags & TH_FIN && tp->snd_nxt == tp->snd_max)
+	    || tp->t_state >= TCPS_FIN_WAIT_1
+	    || (!IS_MASTER_TP(tp) && !tp->mp_active))	//XXX test to prevent maps from being added once application has issued a close
+		snd_dsmap = NULL;
+	else
+		snd_dsmap = mp_find_dsmap(tp, tp->snd_nxt);
+
+	if (snd_dsmap) {
+		off -= snd_dsmap->sf_seq_start - tp->snd_una;
+		 map_unsent = TAILQ_FIRST(&tp->t_txdsmaps)->sf_seq_start
+			+ TAILQ_FIRST(&tp->t_txdsmaps)->ds_map_len
+			- tp->snd_max;
+	} else {
+		map_unsent = 0;
+	}
+
 	/*
 	 * If in persist timeout with window of 0, send 1 byte.
 	 * Otherwise, if window is small but nonzero
@@ -320,7 +718,8 @@
 			 * to send then the probe will be the FIN
 			 * itself.
 			 */
-			if (off < so->so_snd.sb_cc)
+			if ((off < snd_dsmap->ds_map_len) && snd_dsmap ==
+				TAILQ_FIRST(&tp->t_txdsmaps))
 				flags &= ~TH_FIN;
 			sendwin = 1;
 		} else {
@@ -346,27 +745,26 @@
 	 */
 	if (sack_rxmit == 0) {
 		if (sack_bytes_rxmt == 0)
-			len = ((long)ulmin(so->so_snd.sb_cc, sendwin) - off);
+			len = (long)ulmin(map_unsent, sendwin);
 		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)
@@ -392,7 +790,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;
 	}
@@ -421,6 +820,9 @@
 	/* 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__));
+
 	/*
 	 * Automatic sizing of send socket buffer.  Often the send buffer
 	 * size is not optimally adjusted to the actual network conditions
@@ -456,17 +858,18 @@
 	 * 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) {
-		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))) {
-			if (!sbreserve_locked(&so->so_snd,
-			    min(so->so_snd.sb_hiwat + V_tcp_autosndbuf_inc,
-			     V_tcp_autosndbuf_max), so, curthread))
-				so->so_snd.sb_flags &= ~SB_AUTOSIZE;
-		}
-	}
+	//XXXNJW: To move into scheduler or elsewhere
+//	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))) {
+//			if (!sbreserve_locked(&so->so_snd,
+//				min(so->so_snd.sb_hiwat + V_tcp_autosndbuf_inc,
+//				 V_tcp_autosndbuf_max), so, curthread))
+//				so->so_snd.sb_flags &= ~SB_AUTOSIZE;
+//		}
+//	}
 
 	/*
 	 * Decide if we can use TCP Segmentation Offloading (if supported by
@@ -496,15 +899,21 @@
 	    tp->t_inpcb->in6p_options == NULL)
 		tso = 1;
 
+	//XXXNJW: snd_max or snd_max-1?
 	if (sack_rxmit) {
-		if (SEQ_LT(p->rxmit + len, tp->snd_una + so->so_snd.sb_cc))
+		if (SEQ_LT(p->rxmit + len, tp->snd_max))
 			flags &= ~TH_FIN;
 	} else {
-		if (SEQ_LT(tp->snd_nxt + len, tp->snd_una + so->so_snd.sb_cc))
+		if (SEQ_LT(tp->snd_nxt + len, tp->snd_max))
 			flags &= ~TH_FIN;
 	}
 
-	recwin = sbspace(&so->so_rcv);
+	// XXXNJW: Call into mp layer to get the receive window size we want
+	// to advertise. currently does nothing and we just set recwin to
+	// the max window we can advertise (as there is no bound on the
+	// size of the receive buffer).
+	recwin = (long)TCP_MAXWIN << tp->rcv_scale;
+	mp_get_recwin(&recwin);
 
 	/*
 	 * Sender silly window avoidance.   We transmit under the following
@@ -525,12 +934,9 @@
 		 * 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;
 		}
@@ -579,8 +985,9 @@
 			goto dontupdate;
 		if (adv >= (long) (2 * tp->t_maxseg))
 			goto send;
-		if (2 * adv >= (long) so->so_rcv.sb_hiwat)
-			goto send;
+		//XXX To be removed (next merge with head will take care of this)
+		//if (2 * adv >= (long) so->so_rcv.sb_hiwat)
+		//	goto send;
 	}
 dontupdate:
 
@@ -636,7 +1043,7 @@
 	 * 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);
@@ -646,11 +1053,9 @@
 	 * 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.
@@ -675,6 +1080,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;
@@ -686,6 +1093,147 @@
 			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 */
+					tp->sent_capable = 1;
+					tp->mpc = 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();
+					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;
+
+				/* 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;
+			}
+
+			/*
+			 * 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->t_mpcb->mp_flags & MPO_DATA_ACK) {
+				tp->t_mpcb->mp_flags &= ~MPO_DATA_ACK;
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_DATA_ACK;
+				to.to_mopts.mpo_flags |= MPOF_DSS;
+				//to.to_mopts.mpo_flags |= MPOF_ACK64;				// should check against session settings
+				to.to_mopts.data_ack_num =
+						(uint32_t) tp->t_mpcb->ds_rcv_nxt;
+			}
+
+			// XXXNJW: hack testing solution to put DSS on each packet with length
+			// Maps to be provided elsewhere, perhaps using the scheduler?
+//			if (len && tp->mp_active)
+
+			// Only put a map on the first packet
+			if (off == 0 && len)
+				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;
+
+				// build map here.
+				// 32-bit values are copied in add_options
+				//XXX wrapping?
+				to.to_mopts.sub_seq_num = (snd_dsmap->sf_seq_start - tp->iss);
+
+				//XXXNJW detect wrapping
+				to.to_mopts.data_seq_num = (uint32_t) snd_dsmap->ds_map_start;
+
+				to.to_mopts.dss_data_len = snd_dsmap->ds_map_len;
+
+				/* 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))) {
@@ -719,6 +1267,13 @@
 		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;
+		taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->join_task);
+	}
+
 #ifdef INET6
 	if (isipv6)
 		ipoptlen = ip6_optlen(tp->t_inpcb);
@@ -733,6 +1288,7 @@
 	ipoptlen += ipsec_optlen;
 #endif
 
+
 	/*
 	 * Adjust data length if insertion of options will
 	 * bump the packet length beyond the t_maxopd length.
@@ -761,7 +1317,7 @@
 			 * fractional unless the send sockbuf can
 			 * be emptied.
 			 */
-			if (sendalot && off + len < so->so_snd.sb_cc) {
+			if (sendalot && off + len < map_unsent) {
 				len -= len % (tp->t_maxopd - optlen);
 				sendalot = 1;
 			}
@@ -807,9 +1363,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) {
@@ -822,7 +1375,6 @@
 		}
 		MGETHDR(m, M_DONTWAIT, MT_DATA);
 		if (m == NULL) {
-			SOCKBUF_UNLOCK(&so->so_snd);
 			error = ENOBUFS;
 			goto out;
 		}
@@ -830,7 +1382,6 @@
 		if (MHLEN < hdrlen + max_linkhdr) {
 			MCLGET(m, M_DONTWAIT);
 			if ((m->m_flags & M_EXT) == 0) {
-				SOCKBUF_UNLOCK(&so->so_snd);
 				m_freem(m);
 				error = ENOBUFS;
 				goto out;
@@ -841,19 +1392,16 @@
 		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 + off,
+			    (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 + off, (int)len);
 			if (m->m_next == NULL) {
-				SOCKBUF_UNLOCK(&so->so_snd);
 				(void) m_free(m);
 				error = ENOBUFS;
 				goto out;
@@ -861,16 +1409,39 @@
 		}
 
 		/*
+		 * 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 (off + len >= snd_dsmap->ds_map_len) {
+			snd_dsmap->flags |= MAPF_IS_SENT;
+			mp_debug(DSMAP, 4, 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_flags & TF_NEEDFIN &&
+			    snd_dsmap->sf_seq_start + snd_dsmap->ds_map_len == tp->snd_max) // send max + len?
+				snd_dsmap = NULL;
+			else
+				snd_dsmap = mp_find_dsmap(tp, snd_dsmap->sf_seq_start + snd_dsmap->ds_map_len);
+
+			if (snd_dsmap != NULL)
+				map_unsent = snd_dsmap->ds_map_len;
+			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))
@@ -894,7 +1465,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);
@@ -921,6 +1491,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
@@ -998,14 +1569,14 @@
 	 * Calculate receive window.  Don't shrink window,
 	 * but avoid silly window syndrome.
 	 */
-	if (recwin < (long)(so->so_rcv.sb_hiwat / 4) &&
-	    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);
-	if (recwin > (long)TCP_MAXWIN << tp->rcv_scale)
-		recwin = (long)TCP_MAXWIN << tp->rcv_scale;
+//	if (recwin < (long)(so->so_rcv.sb_hiwat / 4) &&
+//	    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);
+//	if (recwin > (long)TCP_MAXWIN << tp->rcv_scale)
+//		recwin = (long)TCP_MAXWIN << tp->rcv_scale;
 
 	/*
 	 * According to RFC1323 the window field in a SYN (i.e., a <SYN>
@@ -1013,8 +1584,9 @@
 	 * case is handled in syncache.
 	 */
 	if (flags & TH_SYN)
-		th->th_win = htons((u_short)
-				(min(sbspace(&so->so_rcv), TCP_MAXWIN)));
+		th->th_win = htons(TCP_MAXWIN);
+//		th->th_win = htons((u_short)
+//				(min(sbspace(&so->so_rcv), TCP_MAXWIN)));
 	else
 		th->th_win = htons((u_short)(recwin >> tp->rcv_scale));
 
@@ -1056,6 +1628,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) {
@@ -1292,7 +1865,6 @@
 				tp->snd_nxt -= len;
 		}
 out:
-		SOCKBUF_UNLOCK_ASSERT(&so->so_snd);	/* Check gotos. */
 		switch (error) {
 		case EPERM:
 			tp->t_softerror = error;
@@ -1359,6 +1931,7 @@
 #endif
 	if (sendalot)
 		goto again;
+
 	return (0);
 }
 
@@ -1481,6 +2054,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 11a8f1275a38 sys/netinet/tcp_reass.c
--- a/sys/netinet/tcp_reass.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_reass.c	Mon Mar 11 02:59:51 2013 +1100
@@ -68,6 +68,8 @@
 #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
@@ -77,6 +79,11 @@
 static int tcp_reass_sysctl_maxseg(SYSCTL_HANDLER_ARGS);
 static int tcp_reass_sysctl_qsize(SYSCTL_HANDLER_ARGS);
 
+#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");
 
@@ -143,13 +150,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));
@@ -169,6 +177,13 @@
 	return (sysctl_handle_int(oidp, arg1, arg2, req));
 }
 
+void
+tcp_reass_release_element(struct tseg_qent *q)
+{
+	uma_zfree(V_tcp_reass_zone, q);
+}
+
+
 int
 tcp_reass(struct tcpcb *tp, struct tcphdr *th, int *tlenp, struct mbuf *m)
 {
@@ -180,9 +195,14 @@
 	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).
@@ -192,8 +212,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
@@ -211,19 +235,21 @@
 	 * Investigate why and re-evaluate the below limit after the behaviour
 	 * is understood.
 	 */
-	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);
-		}
-		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
@@ -244,7 +270,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;
@@ -260,9 +286,23 @@
 	tp->t_segqlen++;
 
 	/*
+	 * 'fastpath' check
+	 */
+	if (th->th_seq == tp->rcv_nxt && !tp->t_segq->disordered) {
+		p = TAILQ_LAST(tp->t_segq, sf_reass_queue_head);
+		goto fastpath;
+	}
+
+	/*
+	 * setting to te is temp until we know exactly where
+	 * disorder starts
+	 */
+	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;
@@ -285,12 +325,10 @@
 				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;	/* ??? */
+				goto unlock_return;
 			}
 			m_adj(m, i);
 			*tlenp -= i;
@@ -315,54 +353,223 @@
 			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; // XXX: Can there be a legitimate 0 DSN?
+
+	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);
+		/* Queue is empty so insert at head. */
+		TAILQ_INSERT_HEAD(tp->t_segq, te, tqe_q);
+		if (th->th_seq == tp->rcv_nxt)
+			tp->t_segq->disordered = NULL;
 	} else {
+		/* Otherwise put it in sequence order in queue. */
 		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 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;
+				}
+			}
+			if (q == NULL)
+				tp->t_segq->disordered = NULL;
+		}
 	}
 
 present:
 	/*
 	 * 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(mp->direct_map)
+		mp->ds_rcv_nxt = tp->rcv_nxt;
+
+	/*
+	 * 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)
+			tp->rcv_nxt = nq->tqe_th->th_seq + nq->tqe_len;
+		else
+			goto unlock_return;	// in this else the first element of the list is disordered, return immediately
+
+	} 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;
+	}
+
+	/* is this segment next in data sequence order? */
+	q = TAILQ_FIRST(tp->t_segq);
+
+	KASSERT(q->tqe_dsn >= tp->t_mpcb->ds_rcv_nxt || q->tqe_th->th_flags & TH_FIN,
+	    ("%s: Segment DSN %ju less than ds_rcv_nxt %ju\n", __func__,
+	    (uintmax_t)q->tqe_dsn,
+	    (uintmax_t)tp->t_mpcb->ds_rcv_nxt));
+
+	// XXXNJW: be careful about reading ds_rcv_nxt
+	if ((mp->direct_map && (q->tqe_th->th_seq + q->tqe_len != tp->rcv_nxt))
+	    || (!mp->direct_map && q->tqe_dsn != tp->t_mpcb->ds_rcv_nxt)) {
+		goto unlock_return;
+	}
+
+	SF_REASS_RUNLOCK(mp);
+
+	/*
+	 * 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 (so->so_rcv.sb_upcall != NULL)
+		mp_do_reass(so);
+
+	//XXXNJW: Does this belong here or in do_reass context?
 	ND6_HINT(tp);
 	sorwakeup_locked(so);
+
+	return (flags);
+
+unlock_return:
+	SF_REASS_RUNLOCK(mp);
 	return (flags);
 }
+
+void
+mp_do_reass(struct socket *so) {
+
+//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 tcpcb *tp = sototcpcb(so); //= ((struct tcpcb *)(ip)->inp_ppcb);
+	struct mpcb *mp;
+	struct tseg_qent *nq = NULL;
+	struct tseg_qent *q = NULL;
+	struct sockbuf *sb;
+
+	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__));
+
+	/*
+	 * 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
+	 */
+	SF_REASS_WLOCK(mp);
+
+	struct tcpcb *subflow_tp = NULL;
+start_reass_head_list:
+	SLIST_FOREACH(subflow_tp, &mp->tp_list, next_subflow) {
+		q = TAILQ_FIRST(subflow_tp->t_segq);
+		if (!q)
+			continue;
+
+		if (q->tqe_dsn == mp->ds_rcv_nxt) {
+			KASSERT(!subflow_tp->t_segq->disordered,
+					("%s Subflow out of order but first element is at ds_rcv_nxt\n", __func__));
+			do {
+				mp->ds_rcv_nxt += q->tqe_len;
+				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--;
+				q = nq;
+			} 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);
+			uma_zfree(V_tcp_reass_zone, q);
+			subflow_tp->t_segqlen--;
+			/* XXX: 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);
+
+	/*
+	 * Schedule a data level ACK. (currently this just sends the
+	 * ACK on the next outbound packet on any subflow
+	 */
+	if (!tp->t_mpcb->direct_map)
+		tp->t_mpcb->mp_flags |= MPO_DATA_ACK;		//XXXNJW move to the point where we have read in the data
+
+}
+
diff -r 11a8f1275a38 sys/netinet/tcp_sack.c
--- a/sys/netinet/tcp_sack.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_sack.c	Mon Mar 11 02:59:51 2013 +1100
@@ -114,6 +114,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
diff -r 11a8f1275a38 sys/netinet/tcp_subr.c
--- a/sys/netinet/tcp_subr.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_subr.c	Mon Mar 11 02:59:51 2013 +1100
@@ -45,6 +45,7 @@
 #include <sys/kernel.h>
 #include <sys/khelp.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 #include <sys/jail.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
@@ -53,6 +54,7 @@
 #endif
 #include <sys/priv.h>
 #include <sys/proc.h>
+#include <sys/queue.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/protosw.h>
@@ -89,6 +91,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
@@ -114,6 +118,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;
@@ -251,6 +259,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)
 
@@ -311,6 +326,10 @@
 	    NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
 	uma_zone_set_max(V_tcpcb_zone, maxsockets);
 
+	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);
+
 	tcp_tw_init();
 	syncache_init();
 	tcp_hc_init();
@@ -341,6 +360,8 @@
 	TUNABLE_INT_FETCH("net.inet.tcp.soreceive_stream", &tcp_soreceive_stream);
 	if (tcp_soreceive_stream) {
 #ifdef INET
+		tcp_usrreqs.pru_soreceive = soreceive_mpstream;
+#else
 		tcp_usrreqs.pru_soreceive = soreceive_stream;
 #endif
 #ifdef INET6
@@ -668,6 +689,7 @@
 {
 	struct tcpcb_mem *tm;
 	struct tcpcb *tp;
+
 #ifdef INET6
 	int isipv6 = (inp->inp_vflag & INP_IPV6) != 0;
 #endif /* INET6 */
@@ -726,6 +748,21 @@
 		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);
+
+	/*
+	 * 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);
+
+	/* 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
@@ -748,6 +785,131 @@
 	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->mp_state = MPS_CLOSED;
+	mp->direct_map = 1;
+	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);
+
+	/* Init task handling for asynchronous sending of MP_JOINs */
+	TASK_INIT(&mp->join_task, 0, mp_join_task_handler, mp);
+
+	TASK_INIT(&mp->data_task, 0, mp_datascheduler_task_handler, mp);
+
+	/*
+	 * Set the masks. We clear bits as ADD_ADDRs and JOINS are sent
+	 * for each of the  slave-subflows.
+	 */
+	uint32_t bitmask = 0xffffffff;
+	mp->mp_advjoin_mask = mp->mp_advaddr_mask = ~(bitmask << mp_address_count);
+
+	/*
+	 * 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);
+
+	/* insert endpoint id of first subflow into local address array */
+	/* use addressID of 0 for the master subflow? */
+	//mp_add_local_addr(mp, (struct in_addr *) &inp->inp_laddr, inp->inp_lport, 0);
+	return (mp);		/* XXX */
+
+}
+
+/*
+ * Free everything. locks, lists etc etc
+ *
+ */
+void
+mp_discardcb(struct mpcb *mp)
+{
+	/* XXXNJW: as we dont remove slave subflows from the list as we
+	 * end them this following check doesn't work. As the master subflow
+	 * is last in the list this should only be called once all the other
+	 * subflows are finished.
+	 */
+//	KASSERT(SLIST_NEXT(SLIST_FIRST(&mp->tp_list), next_subflow) == NULL,
+//	   ("%s: Freeing master tp while slave subflows exist\n", __func__));
+
+
+    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;
+    }
+
+    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);
+
+}
+
+// XXXNJW: To be inlined, renamed
+void
+tcp_ackmap(struct tcpcb *tp, struct tcphdr *th)
+{
+	struct ds_map *map;
+	int completed_a_map = 0;
+
+	do {
+		map = TAILQ_LAST(&tp->t_txdsmaps, dsmapq_head);
+		if (map != NULL && th->th_ack >= (map->sf_seq_start + map->ds_map_len)) {
+			KASSERT(map->flags & MAPF_IS_SENT, ("map %p acked but not marked as sent", map));
+//			mp_debug(DSMAP, 4, "%s: Marked snd_dsmap %p as ACKED, map_len: %d, mbuf_start: %p, sf_seq_start: %u\n",
+//				__func__, map, map->ds_map_len, map->mbuf_start, map->sf_seq_start);
+			/* 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;
+			completed_a_map++;
+			if (tp->t_mpcb->direct_map && map->ds_map_start == tp->t_mpcb->ds_snd_una) {
+				tp->t_mpcb->ds_snd_una += map->ds_map_len;
+			}
+		} else
+			map = NULL; // TAILQ empty or no more maps in the list are covered by this ack
+	} while (map != NULL);
+
+	if (tp->t_mpcb->direct_map && completed_a_map) {
+//		printf("%s: header ACK num %u\n", __func__, th->th_ack);
+		mp_deferred_sbdrop(tp->t_mpcb);
+	}
+}
+
 /*
  * Switch the congestion control algorithm back to NewReno for any active
  * control blocks using an algorithm which is about to go away.
@@ -941,6 +1103,9 @@
 
 	khelp_destroy_osd(tp->osd);
 
+	if (tp->t_mpcb != NULL && IS_MASTER_TP(tp))
+		mp_discardcb(tp->t_mpcb);
+
 	CC_ALGO(tp) = NULL;
 	inp->inp_ppcb = NULL;
 	tp->t_inpcb = NULL;
diff -r 11a8f1275a38 sys/netinet/tcp_syncache.c
--- a/sys/netinet/tcp_syncache.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_syncache.c	Mon Mar 11 02:59:51 2013 +1100
@@ -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>
@@ -54,6 +55,7 @@
 #include <sys/socketvar.h>
 #include <sys/syslog.h>
 #include <sys/ucred.h>
+#include <sys/queue.h>
 
 #include <vm/uma.h>
 
@@ -75,11 +77,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>
@@ -132,6 +138,11 @@
 		*syncookie_lookup(struct in_conninfo *, struct syncache_head *,
 		    struct syncache *, struct tcpopt *, struct tcphdr *,
 		    struct socket *);
+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.
@@ -640,7 +651,9 @@
 {
 	struct inpcb *inp = NULL;
 	struct socket *so;
+	struct mpcb *mp;
 	struct tcpcb *tp;
+
 	int error;
 	char *s;
 
@@ -806,9 +819,46 @@
 #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;
+	/* should have a sysctl to check if we want to enable */
+
 	tp->t_state = TCPS_SYN_RECEIVED;
 	tp->iss = sc->sc_iss;
 	tp->irs = sc->sc_irs;
+
+	if (sc->is_master_subflow) {
+		tp->t_mpcb->direct_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_mp_local_key);
+			mp_process_remote_key(tp->t_mpcb, sc->sc_mp_remote_key);
+		}
+	}
+
+	/* initialise mutex for control block */
+	mtx_init(&mp->mpcb_mutex, "mpcb", NULL, MTX_DEF);
+
 	tcp_rcvseqinit(tp);
 	tcp_sendseqinit(tp);
 	tp->snd_wl1 = sc->sc_irs;
@@ -892,6 +942,165 @@
 	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;
+
+	/*
+	 * Set the socket to the master socket for the mptcp connection.
+	 * The inp has a back pointer to this set when it is created
+	 *
+	 * The process for subflows is a little different to that of
+	 * a primary subflow/traditional tcp connection. We have already
+	 * allocated a new inp for the subflow, and do not want to create
+	 * a new socket based on the listen socket.
+	 *
+	 * Also the tp has already been attached (tcp_attach) to the inp,
+	 * so it should be safe to use inptotcpcb(inp) as the tcpcb is
+	 * already allocated.
+	 *
+	 */
+	so = inp->inp_socket;
+
+	/* unlocking the master inp */
+	INP_WUNLOCK(inp);
+
+//	SOCK_LOCK(so);
+
+	/* returns locked subflow inpcb */
+	inp = in_pcballoc_subflow(so, &V_tcbinfo);
+
+	error = tcp_subflow_attach(so, inp, mp);
+	if (error) {
+		in_pcbfree(inp);
+		goto abort;
+	}
+
+//	SOCK_UNLOCK(so);
+
+	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;
+
+	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);
+
+abort:
+//	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
@@ -1009,7 +1218,149 @@
 	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, to, th, *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
+		V_tcp_syncache.cache_count--;
+		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;
+	}
+
+	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);
@@ -1069,8 +1420,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))
@@ -1277,6 +1629,35 @@
 	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_mp_remote_key = to->to_mopts.remote_key;
+			sc->sc_mp_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->is_master_subflow = 0;
+			sc->mpc = 1;
+		}
+	}
+
 	if (to->to_flags & TOF_SACKPERM)
 		sc->sc_flags |= SCF_SACK;
 	if (to->to_flags & TOF_MSS)
@@ -1300,7 +1681,6 @@
 #endif
 	}
 	SCH_UNLOCK(sch);
-
 	/*
 	 * Do a standard 3-way handshake.
 	 */
@@ -1429,11 +1809,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;
@@ -1450,14 +1830,45 @@
 			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_mp_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;
+				to.to_mopts.truncated_MAC = 0;			// XXXNJW should do MAC calculation somewhere
+			}
+		}
 		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;
@@ -1838,3 +2249,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 11a8f1275a38 sys/netinet/tcp_syncache.h
--- a/sys/netinet/tcp_syncache.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_syncache.h	Mon Mar 11 02:59:51 2013 +1100
@@ -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 *);
@@ -76,6 +78,21 @@
 	struct ucred	*sc_cred;		/* cred cache for jail checks */
 
 	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_mp_remote_key;
+	u_int64_t	sc_mp_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 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 */
 };
 
 /*
@@ -87,12 +104,20 @@
 						/* 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	8	/* dwords */
 #define	SYNCOOKIE_LIFETIME	16	/* seconds */
 
+
+/*
+ * Flags for sc_mp_flags
+ */
+#define MPF_MP_CAPABLE	0x01
+#define MPF_MP_JOIN		0x02
+
 struct syncache_head {
 	struct vnet	*sch_vnet;
 	struct mtx	sch_mtx;
diff -r 11a8f1275a38 sys/netinet/tcp_usrreq.c
--- a/sys/netinet/tcp_usrreq.c	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_usrreq.c	Mon Mar 11 02:59:51 2013 +1100
@@ -46,10 +46,15 @@
 #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/kernel.h>
 #include <sys/sysctl.h>
 #include <sys/mbuf.h>
+#include <sys/queue.h>
+
 #ifdef INET6
 #include <sys/domain.h>
 #endif /* INET6 */
@@ -79,10 +84,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>
@@ -170,6 +178,7 @@
 
 	tp = intotcpcb(inp);
 
+
 	if (inp->inp_flags & INP_TIMEWAIT) {
 		/*
 		 * There are two cases to handle: one in which the time wait
@@ -369,7 +378,7 @@
 	if (error == 0) {
 		tp->t_state = TCPS_LISTEN;
 		solisten_proto(so, backlog);
-#ifdef TCP_OFFLOAD
+#ifdef TCP_OFFLOAD		
 		tcp_offload_listen_start(tp);
 #endif
 	}
@@ -382,6 +391,45 @@
 }
 #endif /* INET */
 
+
+/*
+ * Prepare a subflow to accept connections.
+ * XXXNJW: This is currently called as SYNs arrive on
+ * subflow addresses.
+ */
+int
+tcp_do_subflow_listen(struct inpcb *inp, struct thread *td)
+{
+	int error = 0;
+	struct tcpcb *tp = NULL;
+
+	TCPDEBUG0;
+	KASSERT(inp != NULL, ("tcp_do_subflow_listen: inp == NULL"));
+	INP_WLOCK_ASSERT(inp);
+	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+		error = EINVAL;
+		goto out;
+	}
+	tp = intotcpcb(inp);
+	TCPDEBUG1();
+	INP_HASH_WLOCK(&V_tcbinfo);
+	if (error == 0 && inp->inp_lport == 0)
+		error = in_pcbbind(inp, (struct sockaddr *)0, td->td_ucred);
+	else
+		error = in_pcbinshash(inp);
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+	if (error == 0) {
+		tp->t_state = TCPS_LISTEN;
+#ifdef TCP_OFFLOAD
+		tcp_offload_listen_start(tp);
+#endif
+	}
+
+out:
+	TCPDEBUG2(PRU_LISTEN);
+	return (error);
+}
+
 #ifdef INET6
 static int
 tcp6_usr_listen(struct socket *so, int backlog, struct thread *td)
@@ -433,18 +481,103 @@
  * 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 inpcb *master_inp;
+	struct mpcb *mp;
 	struct tcpcb *tp = NULL;
 	struct sockaddr_in *sinp;
 
 	sinp = (struct sockaddr_in *)nam;
 	if (nam->sa_len != sizeof (*sinp))
 		return (EINVAL);
+
+	/*
+	 * Must disallow TCP ``connections'' to multicast addresses.
+	 */
+	if (sinp->sin_family == AF_INET
+	    && IN_MULTICAST(ntohl(sinp->sin_addr.s_addr)))
+		return (EAFNOSUPPORT);
+	if ((error = prison_remote_ip4(td->td_ucred, &sinp->sin_addr)) != 0)
+		return (error);
+
+	TCPDEBUG0;
+
+	//XXXNJW: need to find a more robust method of determining the
+	//subflow inpcb.
+
+	master_inp = sotoinpcb(so);
+	KASSERT(master_inp != NULL, ("tcp_usr_connect: master_inp == NULL"));
+
+	mp = sototcpcb(so)->t_mpcb;
+
+	/*
+	 * If the socket has just been initialised, this is the
+	 * creation of the master subflow, so we can use the macros.
+	 * Else get pcb's of the first subflow in the list (the first
+	 * subflow is the most recently added subflow)
+	 */
+	if (so->so_state == 0) {
+		inp = sotoinpcb(so);
+		tp = intotcpcb(inp);
+		INP_WLOCK(inp);
+	}
+	else {
+		tp = (SLIST_FIRST(&mp->tp_list));
+		KASSERT(tp != NULL, ("tcp_usr_connect: subflow tp == NULL"));
+		inp = tp->t_inpcb;
+		INP_WLOCK_ASSERT(inp); // locked in the join task handler
+	}
+
+	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+		error = EINVAL;
+		goto out;
+	}
+
+	TCPDEBUG1();
+
+	if(so->so_state == 0) {
+		if ((error = tcp_connect(tp, nam, td)) != 0)
+			goto out;
+	} else {
+		if ((error = tcp_subflow_connect(tp, nam, td)) != 0)
+			goto out;
+	}
+
+#ifdef TCP_OFFLOAD
+	if (registered_toedevs > 0 &&
+	    (error = tcp_offload_connect(so, nam)) == 0)
+		goto out;
+#endif
+	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
+	error = tcp_output(tp);
+
+out:
+	TCPDEBUG2(PRU_CONNECT);
+	INP_WUNLOCK(inp);
+	return (error);
+}
+#endif /* INET */
+
+/*
+ * nam is the foreign address. Also set the local
+ */
+int
+mp_accept_subflow_connection(struct inpcb *inp, struct sockaddr *nam, struct thread *td) {
+	int error = 0;
+	//struct inpcb *inp;
+	struct tcpcb *tp;
+	struct sockaddr_in *sinp;
+
+	sinp = (struct sockaddr_in *)nam;
+	if (nam->sa_len != sizeof (*sinp))
+		return (EINVAL);
 	/*
 	 * Must disallow TCP ``connections'' to multicast addresses.
 	 */
@@ -455,7 +588,6 @@
 		return (error);
 
 	TCPDEBUG0;
-	inp = sotoinpcb(so);
 	KASSERT(inp != NULL, ("tcp_usr_connect: inp == NULL"));
 	INP_WLOCK(inp);
 	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
@@ -463,22 +595,17 @@
 		goto out;
 	}
 	tp = intotcpcb(inp);
+
 	TCPDEBUG1();
-	if ((error = tcp_connect(tp, nam, td)) != 0)
-		goto out;
-#ifdef TCP_OFFLOAD
-	if (registered_toedevs > 0 &&
-	    (error = tcp_offload_connect(so, nam)) == 0)
-		goto out;
-#endif
-	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
-	error = tcp_output(tp);
+
+	tp->t_state = TCPS_SYN_RECEIVED;
+
 out:
 	TCPDEBUG2(PRU_CONNECT);
 	INP_WUNLOCK(inp);
 	return (error);
 }
-#endif /* INET */
+
 
 #ifdef INET6
 static int
@@ -577,25 +704,33 @@
 static int
 tcp_usr_disconnect(struct socket *so)
 {
-	struct inpcb *inp;
-	struct tcpcb *tp = NULL;
 	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: inp == NULL"));
+		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);
+		TCPDEBUG1();
+		tcp_disconnect(sf_tp);
+		TCPDEBUG2(PRU_DISCONNECT);
+		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);
 }
@@ -978,19 +1113,20 @@
 	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);
 		TCPDEBUG1();
 		tcp_disconnect(tp);
 		TCPDEBUG2(PRU_CLOSE);
 	}
+
 	if (!(inp->inp_flags & INP_DROPPED)) {
 		SOCK_LOCK(so);
 		so->so_state |= SS_PROTOREF;
@@ -1107,6 +1243,8 @@
 	u_short lport;
 	int error;
 
+	struct mpcb *mp;
+
 	INP_WLOCK_ASSERT(inp);
 	INP_HASH_WLOCK(&V_tcbinfo);
 
@@ -1147,6 +1285,116 @@
 	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->t_mpcb->direct_map = 1;
+
+	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;
+
+		/* initialise mutex for control block */
+		mtx_init(&mp->mpcb_mutex, "mpcb", NULL, MTX_DEF);
+	}
+
+	return 0;
+
+out:
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+	return (error);
+}
+#endif /* INET */
+
+
+/*
+ * Common subroutine to open a TCP connection to remote host specified
+ * by struct sockaddr_in in mbuf *nam.  Call in_pcbbind to assign a local
+ * port number if needed.  Call in_pcbconnect_setup to do the routing and
+ * to choose a local host address (interface).  If there is an existing
+ * incarnation of the same connection in TIME-WAIT state and if the remote
+ * host was sending CC options and if the connection duration was < MSL, then
+ * truncate the previous TIME-WAIT state and proceed.
+ * Initialize connection parameters and enter SYN-SENT state.
+ */
+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);
+	tp->t_state = TCPS_SYN_SENT;
+	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
 	tp->iss = tcp_new_isn(tp);
 	tcp_sendseqinit(tp);
 
@@ -1156,7 +1404,6 @@
 	INP_HASH_WUNLOCK(&V_tcbinfo);
 	return (error);
 }
-#endif /* INET */
 
 #ifdef INET6
 static int
@@ -1219,6 +1466,7 @@
 	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);
 
@@ -1591,8 +1839,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) {
@@ -1602,6 +1851,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) {
@@ -1617,6 +1867,7 @@
 	else
 #endif
 	inp->inp_vflag |= INP_IPV4;
+
 	tp = tcp_newtcpcb(inp);
 	if (tp == NULL) {
 		in_pcbdetach(inp);
@@ -1631,6 +1882,42 @@
 }
 
 /*
+ * 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);
+	}
+	/* Increase subflow count in mp and add to list of subflow blocks */
+	mp->subflow_cnt += 1;
+	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.
@@ -1708,7 +1995,7 @@
 		break;
 
 	case TCPS_ESTABLISHED:
-		tp->t_state = TCPS_FIN_WAIT_1;
+		tp->t_state = TCPS_FIN_WAIT_1;			//XXXNJW should be set after all DS-level is acked
 		break;
 
 	case TCPS_CLOSE_WAIT:
@@ -1924,7 +2211,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 11a8f1275a38 sys/netinet/tcp_var.h
--- a/sys/netinet/tcp_var.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/netinet/tcp_var.h	Mon Mar 11 02:59:51 2013 +1100
@@ -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,45 @@
 #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;
+	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 */
+//	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);
+
 /*
  * 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 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 +225,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 */
@@ -211,6 +243,20 @@
 	uint32_t t_ispare[8];		/* 5 UTO, 3 TBD */
 	void	*t_pspare2[4];		/* 4 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 */
+
+	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;
 };
 
 /*
@@ -230,6 +276,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 */
@@ -245,6 +292,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
@@ -281,6 +329,40 @@
 #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 */
+	uint8_t	addr_id;		/* address id, used in mp_join */
+	char	snd_mac[20];	/* 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.
@@ -297,7 +379,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 */
@@ -306,6 +389,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 */
 };
 
 /*
@@ -642,6 +726,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 *
@@ -669,6 +762,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
@@ -736,6 +830,20 @@
 
 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 socket *so);
+void	tcp_ackmap(struct tcpcb *tp, struct tcphdr *th);
+
 #endif /* _KERNEL */
 
 #endif /* _NETINET_TCP_VAR_H_ */
diff -r 11a8f1275a38 sys/sys/sockbuf.h
--- a/sys/sys/sockbuf.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/sys/sockbuf.h	Mon Mar 11 02:59:51 2013 +1100
@@ -52,6 +52,7 @@
 #define	SB_NOCOALESCE	0x200		/* don't coalesce new data into existing mbufs */
 #define	SB_IN_TOE	0x400		/* socket buffer is in the middle of an operation */
 #define	SB_AUTOSIZE	0x800		/* automatically size socket buffer */
+#define SB_DEFERACCT 0x1000		/* 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 */
@@ -89,7 +90,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 */
@@ -158,6 +160,10 @@
 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
@@ -170,7 +176,10 @@
 
 /* adjust counters in sb reflecting allocation of m */
 #define	sballoc(sb, m) { \
+	(sb)->sb_actual += (m)->m_len; \
 	(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; \
@@ -183,7 +192,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 11a8f1275a38 sys/sys/socketvar.h
--- a/sys/sys/socketvar.h	Mon Jul 16 20:17:19 2012 +0000
+++ b/sys/sys/socketvar.h	Mon Mar 11 02:59:51 2013 +1100
@@ -340,11 +340,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);
