diff -r 1d1c4c997b66 sys/amd64/conf/MPTCP
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/amd64/conf/MPTCP	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,15 @@
+include		GENERIC
+
+options		KDB
+options		DDB
+
+options		ALQ
+
+options		BREAK_TO_DEBUGGER
+options 	ALT_BREAK_TO_DEBUGGER
+
+options 	IPFIREWALL
+options 	IPFIREWALL_DEFAULT_TO_ACCEPT
+
+nooptions	INET6
+nooptions	IPSEC
diff -r 1d1c4c997b66 sys/conf/files
--- a/sys/conf/files	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/conf/files	Sun Aug 30 14:27:42 2015 +1000
@@ -3474,6 +3474,11 @@
 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/mptcp_timer.c 		optional inet | inet6
+netinet/mptcp_pcb.c 		optional inet | inet6
+netinet/mptcp_usrreq.c 		optional inet
+netinet/mptcp_handshake.c 	optional inet
 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 1d1c4c997b66 sys/conf/kern.pre.mk
--- a/sys/conf/kern.pre.mk	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/conf/kern.pre.mk	Sun Aug 30 14:27:42 2015 +1000
@@ -47,9 +47,9 @@
 .endif
 .if ${MACHINE_CPUARCH} == "amd64"
 .if ${COMPILER_TYPE} == "clang"
-COPTFLAGS?=-O2 -pipe
+COPTFLAGS?=-O0 -pipe
 .else
-COPTFLAGS?=-O2 -frename-registers -pipe
+COPTFLAGS?=-O0 -frename-registers -pipe
 .endif
 .else
 COPTFLAGS?=${_MINUS_O} -pipe
diff -r 1d1c4c997b66 sys/kern/uipc_socket.c
--- a/sys/kern/uipc_socket.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/kern/uipc_socket.c	Sun Aug 30 14:27:42 2015 +1000
@@ -110,6 +110,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/fcntl.h>
+#include <sys/kdb.h>
 #include <sys/limits.h>
 #include <sys/lock.h>
 #include <sys/mac.h>
@@ -126,6 +127,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>
@@ -141,6 +143,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>
@@ -671,6 +678,11 @@
 	return (so);
 }
 
+struct socket *
+gsoalloc(struct vnet *vnet) {
+	return soalloc(vnet);
+}
+
 int
 sobind(struct socket *so, struct sockaddr *nam, struct thread *td)
 {
@@ -832,6 +844,7 @@
 	sodealloc(so);
 }
 
+
 /*
  * Close a socket on last file table reference removal.  Initiate disconnect
  * if connected.  Free socket when disconnect complete.
@@ -1393,6 +1406,7 @@
 	error = so->so_proto->pr_usrreqs->pru_sosend(so, addr, uio, top,
 	    control, flags, td);
 	CURVNET_RESTORE();
+
 	return (error);
 }
 
@@ -2028,6 +2042,7 @@
 	goto restart;
 
 deliver:
+
 	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
 	KASSERT(sbavail(sb) > 0, ("%s: sockbuf empty", __func__));
 	KASSERT(sb->sb_mb != NULL, ("%s: sb_mb == NULL", __func__));
@@ -2125,6 +2140,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_pcb_ref.mp;
+//	KASSERT(mp != NULL, ("%s Doing mpstream re-assembly without a valid mp block\n" ,__func__));
+//
+//	/* We only do stream sockets. */
+//	if (so->so_type != SOCK_STREAM)
+//		return (EINVAL);
+//	if (psa != NULL)
+//		*psa = NULL;
+//	if (controlp != NULL)
+//		return (EINVAL);
+//	if (flagsp != NULL)
+//		flags = *flagsp &~ MSG_EOR;
+//	else
+//		flags = 0;
+//	if (flags & MSG_OOB)
+//		return (soreceive_rcvoob(so, uio, flags));
+//	if (mp0 != NULL)
+//		*mp0 = NULL;
+//
+//	sb = &so->so_rcv;
+//
+//	/* Prevent other readers from entering the socket. */
+//	error = sblock(sb, SBLOCKWAIT(flags));
+//	if (error)
+//		goto out;
+//	SOCKBUF_LOCK(sb);
+//
+//	//mp_do_reass(so);
+//
+//	/* Easy one, no space to copyout anything. */
+//	if (uio->uio_resid == 0) {
+//		error = EINVAL;
+//		goto out;
+//	}
+//	oresid = uio->uio_resid;
+//
+//	/* We will never ever get anything unless we are or were connected. */
+//	if (!(so->so_state & (SS_ISCONNECTED|SS_ISDISCONNECTED))) {
+//		error = ENOTCONN;
+//		goto out;
+//	}
+//
+//restart:
+//	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
+//
+//	/* Abort if socket has reported problems. */
+//	if (so->so_error) {
+//		if (sb->sb_cc > 0)
+//			goto deliver;
+//		if (oresid > uio->uio_resid)
+//			goto out;
+//		error = so->so_error;
+//		if (!(flags & MSG_PEEK))
+//			so->so_error = 0;
+//		goto out;
+//	}
+//
+//	/* Door is closed.  Deliver what is left, if any. */
+//	if (sb->sb_state & SBS_CANTRCVMORE) {
+//		if (sb->sb_cc > 0)
+//			goto deliver;
+//		else
+//			goto out;
+//	}
+//
+//	/*
+//	 * If the last received sequence is less than what
+//	 * was expected.
+//	 */
+////	if (tp->mp_enabled) {
+////		if ((mp->ds_rcv_nxt - mp->ds_last_rcvd ) >= 0)
+////			goto out;
+////	}
+//
+//	/* Socket buffer is empty and we shall not block. */
+//	if (sb->sb_cc == 0 &&
+//	    ((so->so_state & SS_NBIO) || (flags & (MSG_DONTWAIT|MSG_NBIO)))) {
+//		error = EAGAIN;
+//		goto out;
+//	}
+//
+//	/* Socket buffer got some data that we shall deliver now. */
+//	if (sb->sb_cc > 0 && !(flags & MSG_WAITALL) &&
+//	    ((sb->sb_flags & SS_NBIO) ||
+//	     (flags & (MSG_DONTWAIT|MSG_NBIO)) ||
+//	     sb->sb_cc >= sb->sb_lowat ||
+//	     sb->sb_cc >= uio->uio_resid ||
+//	     sb->sb_cc >= sb->sb_hiwat) ) {
+//		goto deliver;
+//	}
+//
+//	/* On MSG_WAITALL we must wait until all data or error arrives. */
+//	if ((flags & MSG_WAITALL) &&
+//	    (sb->sb_cc >= uio->uio_resid || sb->sb_cc >= sb->sb_lowat))
+//		goto deliver;
+//
+//	/*
+//	 * Wait and block until (more) data comes in.
+//	 * NB: Drops the sockbuf lock during wait.
+//	 */
+//	error = sbwait(sb);
+//	if (error)
+//		goto out;
+//	goto restart;
+//
+//deliver:
+//	SOCKBUF_LOCK_ASSERT(&so->so_rcv);
+//	KASSERT(sb->sb_cc > 0, ("%s: sockbuf empty", __func__));
+//	KASSERT(sb->sb_mb != NULL, ("%s: sb_mb == NULL", __func__));
+//
+//	/* Statistics. */
+//	if (uio->uio_td)
+//		uio->uio_td->td_ru.ru_msgrcv++;
+//
+//	/* Fill uio until full or current end of socket buffer is reached. */
+//	len = min(uio->uio_resid, sb->sb_cc);
+//
+////printf("%s: deliver len %d\n",__func__, len);
+//	if (mp0 != NULL) {
+//		/* Dequeue as many mbufs as possible. */
+//		if (!(flags & MSG_PEEK) && len >= sb->sb_mb->m_len) {
+//			for (*mp0 = m = sb->sb_mb;
+//			     m != NULL && m->m_len <= len;
+//			     m = m->m_next) {
+//				len -= m->m_len;
+//				uio->uio_resid -= m->m_len;
+//				sbfree(sb, m);
+//				n = m;
+//			}
+//			sb->sb_mb = m;
+//			if (sb->sb_mb == NULL)
+//				SB_EMPTY_FIXUP(sb);
+//			n->m_next = NULL;
+//		}
+//		/* Copy the remainder. */
+//		if (len > 0) {
+//			KASSERT(sb->sb_mb != NULL,
+//			    ("%s: len > 0 && sb->sb_mb empty", __func__));
+//
+//			m = m_copym(sb->sb_mb, 0, len, M_NOWAIT);
+//			if (m == NULL)
+//				len = 0;	/* Don't flush data from sockbuf. */
+//			else
+//				uio->uio_resid -= m->m_len;
+//			if (*mp0 != NULL)
+//				n->m_next = m;
+//			else
+//				*mp0 = m;
+//			if (*mp0 == NULL) {
+//				error = ENOBUFS;
+//				goto out;
+//			}
+//		}
+//	} else {
+//		/* NB: Must unlock socket buffer as uiomove may sleep. */
+//
+//		SOCKBUF_UNLOCK(sb);
+//		error = m_mbuftouio(uio, sb->sb_mb, len);
+//		SOCKBUF_LOCK(sb);
+//
+//		if (error)
+//			goto out;
+//	}
+//	SBLASTRECORDCHK(sb);
+//	SBLASTMBUFCHK(sb);
+//
+//	/*
+//	 * Remove the delivered data from the socket buffer unless we
+//	 * were only peeking.
+//	 */
+//	if (!(flags & MSG_PEEK)) {
+//		if (len > 0)
+//			sbdrop_locked(sb, len);
+//
+//		/* Notify protocol that we drained some data. */
+//		if ((so->so_proto->pr_flags & PR_WANTRCVD) &&
+//		    (((flags & MSG_WAITALL) && uio->uio_resid > 0) ||
+//		     !(flags & MSG_SOCALLBCK))) {
+//			SOCKBUF_UNLOCK(sb);
+//			VNET_SO_ASSERT(so);
+//			(*so->so_proto->pr_usrreqs->pru_rcvd)(so, flags);
+//			SOCKBUF_LOCK(sb);
+//		}
+//	}
+//
+//	/*
+//	 * For MSG_WAITALL we may have to loop again and wait for
+//	 * more data to come in.
+//	 */
+//	if ((flags & MSG_WAITALL) && uio->uio_resid > 0)
+//		goto restart;
+//out:
+//
+//	SOCKBUF_LOCK_ASSERT(sb);
+//	SBLASTRECORDCHK(sb);
+//	SBLASTMBUFCHK(sb);
+//	SOCKBUF_UNLOCK(sb);
+//	sbunlock(sb);
+//	return (error);
+//}
+
 /*
  * Optimized version of soreceive() for simple datagram cases from userspace.
  * Unlike in the stream case, we're able to drop a datagram if copyout()
@@ -3049,6 +3283,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;
diff -r 1d1c4c997b66 sys/netinet/in_pcb.c
--- a/sys/netinet/in_pcb.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/in_pcb.c	Sun Aug 30 14:27:42 2015 +1000
@@ -326,6 +326,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)
@@ -1079,6 +1142,7 @@
 		if (error)
 			return (error);
 	}
+
 	oinp = in_pcblookup_hash_locked(inp->inp_pcbinfo, faddr, fport,
 	    laddr, lport, 0, NULL);
 	if (oinp != NULL) {
@@ -1293,7 +1357,6 @@
 void
 in_pcbdrop(struct inpcb *inp)
 {
-
 	INP_WLOCK_ASSERT(inp);
 
 	/*
diff -r 1d1c4c997b66 sys/netinet/in_pcb.h
--- a/sys/netinet/in_pcb.h	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/in_pcb.h	Sun Aug 30 14:27:42 2015 +1000
@@ -632,6 +632,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 1d1c4c997b66 sys/netinet/in_proto.c
--- a/sys/netinet/in_proto.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/in_proto.c	Sun Aug 30 14:27:42 2015 +1000
@@ -82,6 +82,7 @@
 #include <netinet/udp.h>
 #include <netinet/udp_var.h>
 #include <netinet/ip_encap.h>
+#include <netinet/mptcp_var.h>
 
 /*
  * TCP/IP protocol family: IP, ICMP, UDP, TCP.
@@ -146,14 +147,14 @@
 	.pr_flags =		PR_CONNREQUIRED|PR_IMPLOPCL|PR_WANTRCVD,
 	.pr_input =		tcp_input,
 	.pr_ctlinput =		tcp_ctlinput,
-	.pr_ctloutput =		tcp_ctloutput,
+	.pr_ctloutput =		mp_ctloutput,
 	.pr_init =		tcp_init,
 #ifdef VIMAGE
 	.pr_destroy =		tcp_destroy,
 #endif
 	.pr_slowtimo =		tcp_slowtimo,
 	.pr_drain =		tcp_drain,
-	.pr_usrreqs =		&tcp_usrreqs
+	.pr_usrreqs =		&mptcp_usrreqs
 },
 #ifdef SCTP
 { 
diff -r 1d1c4c997b66 sys/netinet/mptcp.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,93 @@
+/*-
+ * Copyright (c) 2012-2015
+ * 	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 FreeBSD
+ * Foundation and 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 */
+
+
+#define	MPTCP_SA_NAME_MAX	16	/* max scheduler discipline name length */
+
+#endif /* MPTCP_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_dtrace_declare.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_dtrace_declare.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,79 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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 <sys/cdefs.h>
+#ifndef MPTCP_DTRACE_DECLARE_H_
+#define MPTCP_DTRACE_DECLARE_H_
+
+#include "opt_kdtrace.h"
+#include <sys/kernel.h>
+#include <sys/sdt.h>
+
+/* Declare the MPTCP provider */
+SDT_PROVIDER_DECLARE(mptcp);
+
+/* Allocated a MPPCB */
+SDT_PROBE_DECLARE(mptcp, session, mpp_pcballoc, mppcb_alloc);
+
+/* Released a MPPCB */
+SDT_PROBE_DECLARE(mptcp, session, mpp_pcbrele, mppcb_release);
+
+/* Attached a MPCB */
+SDT_PROBE_DECLARE(mptcp, session, mp_attach, mpcb_attached);
+
+/* Discard a MPCB */
+SDT_PROBE_DECLARE(mptcp, session, mp_discardcb, entry);
+
+/* Info on connection becoming established */
+SDT_PROBE_DECLARE(mptcp, session, mp_init_established, estab_info);
+
+/* Setting options at the subflow level. */
+SDT_PROBE_DECLARE(mptcp, session, mp_setopt, entry);
+
+/* Getting options at the subflow level. */
+SDT_PROBE_DECLARE(mptcp, session, mp_getopt, entry);
+
+/* Setting options at the subflow level. */
+SDT_PROBE_DECLARE(mptcp, session, mp_process_subflow_event, connected);
+
+/* Local session key and token */
+SDT_PROBE_DECLARE(mptcp, session, mp_process_local_key, new_key);
+
+/* Foreign Host session key and token */
+SDT_PROBE_DECLARE(mptcp, session, mp_process_remote_key, new_key);
+
+/* Detach tcpcb from socket */
+SDT_PROBE_DECLARE(mptcp, session, tcp_detach, entry);
+
+
+#endif /* MPTCP_DTRACE_DECLARE_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_dtrace_define.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_dtrace_define.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,99 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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 <sys/cdefs.h>
+#ifndef MPTCP_DTRACE_DEFINE_H_
+#define MPTCP_DTRACE_DEFINE_H_
+
+#include "opt_kdtrace.h"
+#include <sys/kernel.h>
+#include <sys/sdt.h>
+
+SDT_PROVIDER_DEFINE(mptcp);
+
+/* Allocated a new mppcb */
+SDT_PROBE_DEFINE1(mptcp, session, mpp_pcballoc, mppcb_alloc,
+	"uintptr_t");    /* Pointer to mppcb struct allocated */
+
+/* Released a mppcb */
+SDT_PROBE_DEFINE1(mptcp, session, mpp_pcbrele, mppcb_release,
+	"uintptr_t");    /* Pointer to mppcb struct to be released */
+
+/* Attached MP transport block */
+SDT_PROBE_DEFINE1(mptcp, session, mp_attach, mpcb_attached,
+	"uintptr_t");    /* Pointer to mpcb struct attached to mppcb */
+
+/* Discard MP transport block */
+SDT_PROBE_DEFINE1(mptcp, session, mp_discardcb, entry,
+	"uintptr_t");    /* Pointer to mpcb struct to be discarded */
+
+/* MP session transitions to M_ESTABLISHED */
+SDT_PROBE_DEFINE5(mptcp, session, mp_init_established, estab_info,
+    "uint64_t",		/* Initial data-sequence send */
+    "uint64_t",		/* Initial data-sequence receive */
+    "uintptr_t",	/* The pointer to the struct mpcb */
+    "uint32_t",	    /* Local mp session token */
+    "uint32_t");	/* Remote mp session token */
+
+/* Setting socket options */
+SDT_PROBE_DEFINE3(mptcp, session, mp_setopt, entry,
+	"uintptr_t",    /* Pointer to mpcb struct */
+	"int_t",    /* Protocol level */
+	"int_t");	/* Option value */
+
+/* Getting socket options */
+SDT_PROBE_DEFINE3(mptcp, session, mp_getopt, entry,
+	"uintptr_t",    /* Pointer to mpcb struct */
+	"int_t",    /* Protocol level */
+	"int_t");	/* Option value */
+
+/* Generation of local key */
+SDT_PROBE_DEFINE2(mptcp, session, mp_process_subflow_event, connected,
+	"uintptr_t",    /* Pointer to mpcb struct */
+	"uintptr_t");   /* Pointer to tcbcb struct */
+
+/* Generation of local key */
+SDT_PROBE_DEFINE2(mptcp, session, mp_process_local_key, new_key,
+    "uint32_t",		/* Local session token */
+    "uint64_t");	/* Local key */
+
+/* Remote key and foreign host session token */
+SDT_PROBE_DEFINE2(mptcp, session, mp_process_remote_key, new_key,
+    "uint32_t",		/* Foreign host session token */
+    "uint64_t");	/* Remote key */
+
+SDT_PROBE_DEFINE1(mptcp, session, tcp_detach, entry,
+		"uintptr_t");   /* Pointer to struct socket */
+
+
+#endif /* MPTCP_DTRACE_DEFINE_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_handshake.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_handshake.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,550 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD 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/protosw.h>
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/protosw.h>
+#include <sys/random.h>
+
+/* for SCTP auth functions */
+#include <crypto/sha1.h>
+
+/* for checking interface status */
+#include <net/if.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_usrreq.h>
+#include <netinet/tcp_fsm.h>
+#include <netinet/tcp_seq.h>
+#include <netinet/tcp_syncache.h>
+
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_dtrace_declare.h>
+
+/* for SCTP auth functions */
+#include <netinet/sctp_pcb.h>
+#include <netinet/sctp_auth.h>
+#include <netinet/sctp_constants.h>
+
+#include <machine/in_cksum.h>
+#include <machine/stdarg.h>
+
+static void mp_instoklist(struct mpcb *mp);
+
+void
+mp_init_established(struct mpcb *mp)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	/* we know the other end is mp enabled */
+	if (mp->mp_state < MPS_M_ESTABLISHED) {
+		mp->mp_state = MPS_M_ESTABLISHED;
+
+		printf("%s: mp %p is established\n", __func__, mp);
+
+		/* mp_connected checked by mp_usrreqs when shutting down connection */
+		mp->mp_connected = 1;
+
+		/* XXXNJW: why not use mp_sendseqinit? */
+		mp->ds_map_max = mp->ds_snd_una = mp->ds_snd_max = mp->ds_map_min =
+			mp->ds_snd_nxt = mp->ds_idsn + 1;
+		mp->ds_rcv_nxt = mp->ds_idsr + 1;
+
+		/* XXXNJW: temp way of inserting tokens into mp-hashlist */
+		mp_instoklist(mp);
+
+		SDT_PROBE(mptcp, session, mp_init_established, estab_info,
+			mp->ds_idsn, mp->ds_idsr, mp, mp->local_token, mp->remote_token);
+	}
+
+}
+
+void
+mp_syncache_process_local_key(struct syncache *sc)
+{
+	uint8_t digest[20];
+	sc->sc_local_key = mp_generate_local_key();
+	mp_do_sha_hash(digest, (uint8_t*) &sc->sc_local_key,
+		 sizeof(sc->sc_local_key));
+	sc->sc_ds_iss = mp_new_idsn(digest);
+	sc->sc_ds_iss = htobe64(sc->sc_ds_iss);
+	sc->sc_mp_local_token = mp_get_token(digest);
+	printf("%s: local token: %u\n", __func__,
+	    sc->sc_mp_local_token);
+}
+
+void
+mp_syncache_process_remote_key(struct syncache *sc, uint64_t remote_key)
+{
+	uint8_t digest[20];
+	sc->sc_remote_key = remote_key;
+	mp_do_sha_hash(digest, (uint8_t*) &sc->sc_remote_key,
+		 sizeof(sc->sc_remote_key));
+	sc->sc_ds_irs = mp_new_idsn(digest);
+	sc->sc_ds_irs = htobe64(sc->sc_ds_irs);
+	sc->sc_mp_remote_token = mp_get_token(digest);
+}
+
+void
+mp_process_local_key(struct mp_connection *mp_conn, uint64_t local_key)
+{
+	uint8_t digest[20];
+	char buf[256];
+
+	mp_do_sha_hash(digest, (uint8_t*) &local_key, sizeof(local_key));
+
+	mp_conn->ds_idss = mp_new_idsn(digest);
+	mp_conn->local_key = local_key;
+
+	/* As linux does this, we need to do it for interop */
+	mp_conn->ds_idss = htobe64(mp_conn->ds_idss);
+	mp_conn->local_token = mp_get_token(digest);
+
+	btohex(buf, sizeof(buf), digest, sizeof(digest), BTOHEX_MSBLTOR);
+	mp_debug(MPSESSION, 4, 0, "SHA1(local_key) = 0x%s\n", buf);
+
+	SDT_PROBE2(mptcp, session, mp_process_local_key, new_key,
+		mp_conn->local_token, mp_conn->local_key);
+}
+
+void
+mp_process_remote_key(struct mp_connection *mp_conn, uint64_t remote_key)
+{
+	uint8_t digest[20];
+	char buf[256];
+
+	mp_do_sha_hash(digest, (uint8_t*) &remote_key, sizeof(remote_key));
+
+	mp_conn->ds_idrs = mp_new_idsn(digest);
+	mp_conn->remote_key = remote_key;
+
+	mp_conn->ds_idrs = htobe64(mp_conn->ds_idrs);
+	mp_conn->remote_token = mp_get_token(digest);
+
+	mp_debug(MPSESSION, 4, 0, "%s: idrs: %ju : %u\n", __func__,
+		(uintmax_t)mp_conn->ds_idrs, (uint32_t)mp_conn->ds_idrs);
+
+	/* debug output from hash */
+	btohex(buf, sizeof(buf), digest, sizeof(digest), BTOHEX_MSBLTOR);
+//	mp_debug(MPSESSION, 4, 0, "SHA1(remote_key) = 0x%s\n", buf);
+//	mp_debug(MPSESSION, 4, 0, "remote idsn = %u\n", (uint32_t)mp_conn->ds_idrs);
+
+	SDT_PROBE2(mptcp, session, mp_process_remote_key, new_key,
+		mp_conn->remote_token, mp_conn->remote_key);
+}
+
+
+/*
+ * Based on syncache_respond. Used to send a SYN/ACK on receipt of MP_JOIN,
+ * as the syncache is not used for JOIN connections. Perhaps a syncache-like
+ * cache of mp_joins would be appropriate?
+ */
+int
+mp_join_respond(struct socket *so, struct tcpcb *tp, struct in_conninfo *inc)
+{
+	struct ip *ip = NULL;
+	struct mbuf *m;
+	struct tcphdr *th;
+	struct tcpopt to;
+	int optlen, error = 0;	/* Make compiler happy */
+	int win, wscale = 0;
+
+	u_int16_t hlen, tlen, mssopt;
+	hlen =	sizeof(struct ip);
+	tlen = hlen + sizeof(struct tcphdr);
+
+	KASSERT((tp->t_sf_flags & SFF_GOT_JOIN_SYN),
+	    ("%s: didn't get a join SYN\n", __func__));
+
+	win = sbspace(&so->so_rcv);
+	win = imax(win, 0);
+	win = imin(win, TCP_MAXWIN);
+
+	tp->rcv_wnd = win;
+
+	if (V_tcp_do_rfc1323) {
+		if (tp->t_flags & TF_RCVD_SCALE) {
+			/*
+			 * Pick the smallest possible scaling factor that
+			 * will still allow us to scale up to sb_max, aka
+			 * kern.ipc.maxsockbuf.
+			 *
+			 * We do this because there are broken firewalls that
+			 * will corrupt the window scale option, leading to
+			 * the other endpoint believing that our advertised
+			 * window is unscaled.  At scale factors larger than
+			 * 5 the unscaled window will drop below 1500 bytes,
+			 * leading to serious problems when traversing these
+			 * broken firewalls.
+			 *
+			 * With the default maxsockbuf of 256K, a scale factor
+			 * of 3 will be chosen by this algorithm.  Those who
+			 * choose a larger maxsockbuf should watch out
+			 * for the compatiblity problems mentioned above.
+			 *
+			 * RFC1323: The Window field in a SYN (i.e., a <SYN>
+			 * or <SYN,ACK>) segment itself is never scaled.
+			 */
+			while (wscale < TCP_MAX_WINSHIFT &&
+				(TCP_MAXWIN << wscale) < sb_max)
+				wscale++;
+		}
+	}
+
+	/* Determine MSS we advertize to other end of connection. */
+	mssopt = tcp_mssopt(inc);
+	if (to.to_mss)
+		mssopt = max( min(to.to_mss, mssopt), V_tcp_minmss);
+
+	/* Create the IP+TCP header from scratch. */
+	m = m_gethdr(M_NOWAIT, MT_DATA);
+	if (m == NULL)
+		return (ENOBUFS);
+	m->m_data += max_linkhdr;
+	m->m_len = tlen;
+	m->m_pkthdr.len = tlen;
+	m->m_pkthdr.rcvif = NULL;
+
+	ip = mtod(m, struct ip *);
+	ip->ip_v = IPVERSION;
+	ip->ip_hl = sizeof(struct ip) >> 2;
+	ip->ip_len = htons(tlen);
+	ip->ip_id = 0;
+	ip->ip_off = 0;
+	ip->ip_sum = 0;
+	ip->ip_p = IPPROTO_TCP;
+	ip->ip_src = inc->inc_laddr;
+	ip->ip_dst = inc->inc_faddr;
+	ip->ip_ttl = tp->t_inpcb->inp_ip_ttl;
+	ip->ip_tos = tp->t_inpcb->inp_ip_tos;
+
+	/*
+	 * See if we should do MTU discovery.  Route lookups are
+	 * expensive, so we will only unset the DF bit if:
+	 *
+	 *	1) path_mtu_discovery is disabled
+	 *	2) the SCF_UNREACH flag has been set
+	 */
+	if (V_path_mtu_discovery && tp->t_rxtshift < 3 + 1)
+        ip->ip_off |= htons(IP_DF);
+
+	th = (struct tcphdr *)(ip + 1);
+	th->th_sport = inc->inc_lport;
+	th->th_dport = inc->inc_fport;
+
+	th->th_seq = htonl(tp->iss);
+	th->th_ack = htonl(tp->irs + 1);
+	th->th_off = sizeof(struct tcphdr) >> 2;
+	th->th_x2 = 0;
+	th->th_flags = TH_SYN|TH_ACK;
+	th->th_win = htons(win);
+	th->th_urp = 0;
+
+	// should also check that we got an ECN flag in the SYN
+	if (V_tcp_do_ecn) {
+		th->th_flags |= TH_ECE;
+		TCPSTAT_INC(tcps_ecn_shs);
+	}
+
+	/* Tack on the TCP options. */
+	if ((tp->t_flags & TF_NOOPT) == 0) {
+		to.to_flags = 0;
+
+		to.to_mss = mssopt;
+		to.to_flags = TOF_MSS;
+		if (tp->t_flags & TF_RCVD_SCALE) {
+			to.to_wscale = wscale;
+			to.to_flags |= TOF_SCALE;
+		}
+		if (tp->t_flags & TF_RCVD_TSTMP) {
+			to.to_tsval = tcp_ts_getticks();
+			to.to_tsecr = tp->ts_recent;
+			to.to_flags |= TOF_TS;
+		}
+		if (tp->t_flags & TF_SACK_PERMIT)
+			to.to_flags |= TOF_SACKPERM;
+
+		/* The MP_JOIN option */
+		to.to_mopts.mpo_flags = 0;
+		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 = tp->t_mp_conn.local_rand;
+
+		/* copy keys to calculate HMAC */
+		to.to_mopts.remote_key = tp->t_mp_conn.remote_key;
+		to.to_mopts.local_key = tp->t_mp_conn.local_key;
+
+        /* XXXNJW: actually not calculating or validating the hmacs for the
+         * moment*/
+
+		optlen = tcp_addoptions(&to, (u_char *)(th + 1));
+
+		/* Adjust headers by option size. */
+		th->th_off = (sizeof(struct tcphdr) + optlen) >> 2;
+		m->m_len += optlen;
+		m->m_pkthdr.len += optlen;
+
+		ip->ip_len = htons(ntohs(ip->ip_len) + optlen);
+
+	} else
+		return 1;  // if NOOPT set, can't actually proceed as we need
+	             // options in order to send the mp_join...
+
+	M_SETFIB(m, inc->inc_fibnum);
+	m->m_pkthdr.csum_data = offsetof(struct tcphdr, th_sum);
+
+	m->m_pkthdr.csum_flags = CSUM_TCP;
+	th->th_sum = in_pseudo(ip->ip_src.s_addr, ip->ip_dst.s_addr,
+		htons(tlen + optlen - hlen + IPPROTO_TCP));
+
+	error = ip_output(m, tp->t_inpcb->inp_options, NULL, 0, NULL, tp->t_inpcb);
+	INP_WUNLOCK(tp->t_inpcb);
+
+	return (error);
+
+}
+
+/* XXXNJW: temp
+ * The token info struct ties a mpcb to session tokens, so that the
+ * mpcb can be located via a session token. In the case where we
+ * don't have memory to allocate this struct, just continue on. The
+ * result will be that MP_JOINs from addresses that have not been
+ * explicitly added will not associate with the session. */
+static void
+mp_instoklist(struct mpcb *mp)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	struct mpcb_tokinfo *tokeninfo;
+	tokeninfo = malloc(sizeof(struct mpcb_tokinfo), M_MPTOKINFO, M_NOWAIT);
+	if (tokeninfo != NULL) {
+		tokeninfo->mpti_pcb = mp;
+		tokeninfo->mpti_local_token = mp->local_token;
+		tokeninfo->mpti_remote_token = mp->remote_token;
+
+		MPTOK_INFO_WLOCK(&mp_tokinfo_list);
+		SLIST_INSERT_HEAD(&mp_tokinfo_list.mpti_listhead, tokeninfo,
+			mpti_entry);
+		MPTOK_INFO_WUNLOCK(&mp_tokinfo_list);
+	}
+}
+
+void
+mp_remtoklist(uint32_t local_token)
+{
+	struct mpcb_tokinfo *tokeninfo;
+
+	MPTOK_INFO_WLOCK(&mp_tokinfo_list);
+	SLIST_FOREACH(tokeninfo, &mp_tokinfo_list.mpti_listhead, mpti_entry) {
+		if (tokeninfo->mpti_local_token == local_token) {
+			printf("%s: free token\n", __func__);
+			SLIST_REMOVE(&mp_tokinfo_list.mpti_listhead,
+			    tokeninfo, mpcb_tokinfo, mpti_entry);
+			free(tokeninfo, M_MPTOKINFO);
+		}
+	}
+	MPTOK_INFO_WUNLOCK(&mp_tokinfo_list);
+}
+
+/* Find and return locked mpcb for a given token */
+struct mpcb*
+mp_locate_mpcb(uint32_t token)
+{
+    struct mpcb *mp = NULL;
+    struct mpcb_tokinfo *tokeninfo;
+
+    MPTOK_INFO_WLOCK(&mp_tokinfo_list);
+	SLIST_FOREACH(tokeninfo, &mp_tokinfo_list.mpti_listhead, mpti_entry) {
+		if (tokeninfo->mpti_local_token == token) {
+			mp = tokeninfo->mpti_pcb;
+			KASSERT(mp != NULL, ("%s: mp NULL\n", __func__));
+			break;
+		}
+	}
+	MPTOK_INFO_WUNLOCK(&mp_tokinfo_list);
+
+	if (mp)
+		MPP_LOCK(mp->mp_mppcb);
+
+	return(mp);
+}
+
+/*
+ * Some basic random number/hashing functions that should
+ * be okay for initial release
+ *
+ * XXXNJW: Need a more secure method?
+ */
+u_int64_t
+mp_generate_local_key(void)
+{
+       u_int64_t key = arc4random();
+       key = key << 32;
+       key += arc4random();
+       return key;
+}
+
+/*
+ * Digest is for a 20 byte SHA1, key is the session key created
+ * when the mpcb was created
+ */
+uint32_t
+mp_do_sha_hash(uint8_t *digest, uint8_t *key, uint64_t key_length) {
+
+       SHA1_CTX context;
+
+       if ((key == NULL) || (key_length == 0)) {
+              return (0);
+       }
+
+       SHA1Init(&context);
+       SHA1Update(&context, key, key_length);
+       SHA1Final(digest, &context);
+
+       return key_length;
+}
+
+// XXXNJW: turn these into Macros
+/*
+ * Take a digest and return a token. Linux_compat
+ * changes the offset into the digest.
+ */
+uint32_t
+mp_get_token(uint8_t *digest) {
+//	if(!linux_compat)
+//		return (*((uint32_t *) (digest + 16)));
+//	else
+		return (*((uint32_t *) (digest)));
+}
+
+/*
+ * Take a digest and return the idsn. Linux_compat
+ * changes the offset into the digest.
+ */
+
+uint64_t
+mp_new_idsn(uint8_t *digest) {
+//	if(!linux_compat)
+//		return (*((uint64_t *) (digest)));
+//	else
+		return (*((uint64_t *) (digest+12)));
+}
+
+/*
+ * HMAC for used when completing mp_join handshake.
+ * We use the hmac functions implemented in sctp_auth.c
+ */
+uint32_t
+mp_get_hmac(uint8_t *digest, uint64_t local_key, uint64_t remote_key,
+		uint32_t local_rand, uint32_t remote_rand) {
+	/*
+	* Set up the hmac key
+	*/
+	uint8_t key[16];
+	uint32_t keylen = sizeof(local_key) + sizeof(remote_key);
+
+	bcopy(&local_key, key, sizeof(local_key));
+	bcopy(&remote_key, key + 8, sizeof(remote_key));
+
+	/*
+	* Set up the hmac msg
+	*/
+	uint64_t hmac_msg;
+	hmac_msg = (uint64_t)local_rand << 32;
+	hmac_msg |= remote_rand;
+
+	/*
+	 * Do the hashing. 0x0001 is the type for sha1
+	 */
+	uint32_t hmac_len = sctp_hmac(0x0001, key, keylen,
+	    (uint8_t *) &hmac_msg, sizeof(hmac_msg), digest);
+
+	return(hmac_len);
+}
+
+/*
+ * Debugging function for printing a sequence of bytes
+ */
+void
+btohex(char *buf, uint32_t buf_len, uint8_t *bytes, int32_t bytes_len, int32_t flags)
+{
+	struct sbuf out;
+	int32_t i;
+
+	i = bytes_len - 1;
+	sbuf_new(&out, buf, buf_len, 0);
+
+	while (i >= 0) {
+		if (flags & BTOHEX_MSBRTOL)
+			sbuf_printf(&out, "%0x", *(bytes + bytes_len - i - 1));
+		else
+			sbuf_printf(&out, "%0x", *(bytes + i));
+		i--;
+	}
+
+	sbuf_finish(&out);
+}
diff -r 1d1c4c997b66 sys/netinet/mptcp_pcb.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_pcb.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,232 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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/systm.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/refcount.h>
+#include <sys/rwlock.h>
+#include <sys/sysctl.h>
+#include <sys/sbuf.h>
+#include <sys/jail.h>
+
+#include <sys/priv.h>
+#include <sys/proc.h>
+#include <sys/protosw.h>
+
+/* XXXNJW: too many header dependencies with mptcp_var.h? (i.e. need to pull in
+ * in.h etc etc to compile. */
+#include <netinet/in.h>
+
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+
+#include <net/vnet.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_dtrace_define.h>
+
+static VNET_DEFINE(uma_zone_t, mppcb_zone);
+#define	V_mppcb_zone			VNET(mppcb_zone)
+
+/*
+ * Allocate a PCB and associate it with the socket.
+ * On success return with the PCB locked.
+ */
+int
+mpp_pcballoc(struct socket *so)
+{
+	struct mppcb *mpp;
+
+	mpp = uma_zalloc(V_mppcb_zone, M_NOWAIT | M_ZERO);
+	if (mpp == NULL)
+		return (ENOBUFS);
+	mpp->mpp_socket = so;
+	mpp->mpp_cred = crhold(so->so_cred);
+	mpp->mpp_fibnum = so->so_fibnum;
+	so->so_pcb = (caddr_t)mpp;
+	refcount_init(&mpp->mpp_refcount, 1);
+
+	MPP_LOCK_INIT(mpp);
+	MPP_LOCK(mpp);
+
+	SDT_PROBE1(mptcp, session, mpp_pcballoc, mppcb_alloc, mpp);
+
+	return (0);
+}
+
+int
+mpp_getsockaddr(struct socket *so, struct sockaddr **nam)
+{
+	struct mppcb *mpp;
+	uint32_t addr;
+	in_port_t port;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mpp_getsockaddr: mp == NULL"));
+
+	MPP_LOCK(mpp);
+	port = mpp->mpp_lport;
+	addr = mpp->mpp_laddr;
+	MPP_UNLOCK(mpp);
+
+	*nam = in_sockaddr(port, (struct in_addr*) &addr);
+	return 0;
+}
+
+
+/* should have a primary inp if this function is called. */
+int
+mpp_getpeeraddr(struct socket *so, struct sockaddr **nam)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	struct sf_handle *sfh;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("in_getpeeraddr: inp == NULL"));
+	MPP_LOCK(mpp);
+
+    mp = mpptompcb(mpp);
+    KASSERT(mp != NULL, ("in_getsockaddr: mp == NULL"));
+
+    /* XXXNJW: Just using the first subflow for now */
+    sfh = TAILQ_FIRST(&mp->sf_list);
+    KASSERT(sfh->sf_so != NULL, ("in_getpeeraddr: sf_so == NULL"));
+
+  	MPP_UNLOCK(mpp);
+
+	/* not good, but temporary */
+    (*(sfh->sf_so)->so_proto->pr_usrreqs->pru_peeraddr)(sfh->sf_so, nam);
+
+	return 0;
+}
+
+
+/*
+ * mpp_pcbdetach() is responsible for dissociating a socket from an mppcb.
+ * With established MPTCP connections, the mppcb may significantly outlive
+ * the socket, in which case mpp_pcbfree() is deferred.
+ */
+void
+mpp_pcbdetach(struct mppcb *mpp)
+{
+	KASSERT(mpp->mpp_socket != NULL, ("%s: mpp_socket == NULL", __func__));
+
+	mpp->mpp_socket->so_pcb = NULL;
+	mpp->mpp_socket = NULL;
+}
+
+void
+mpp_pcbdrop(struct mppcb *mpp)
+{
+	MPP_LOCK_ASSERT(mpp);
+	mpp->mpp_flags |= MPP_DROPPED;
+}
+
+void
+mpp_pcbfree(struct mppcb *mpp)
+{
+	KASSERT(mpp->mpp_socket == NULL, ("%s: mpp_socket != NULL", __func__));
+
+	MPP_LOCK_ASSERT(mpp);
+
+	if (!mpp_pcbrele(mpp))
+	    MPP_UNLOCK(mpp);
+}
+
+/* XXXNJW - comment to reflect what happens in the case of
+ * an mppcb refcount (much the same as with an inpcb count,
+ * but there are some odd uses of the refcount in the code
+ * currently (see tcp_do_segement and goto mp_input within) */
+void
+mpp_pcbref(struct mppcb *mpp)
+{
+	KASSERT(mpp->mpp_refcount > 0, ("%s: refcount 0", __func__));
+	refcount_acquire(&mpp->mpp_refcount);
+}
+
+/* XXXNJW: This is a workaround to allow tcp_do_segment
+ * to release a reference on the mpp without already
+ * holding the MPP_LOCK. Currently tcp_do_segment bumps
+ * the refcount on the mpp to prevent a use-after-free
+ * error on connection closes where pru_close and tcp
+ * shutdown race with one another. */
+void
+mpp_pcbrele_unlocked(struct mppcb *mpp)
+{
+	MPP_LOCK(mpp);
+	if(!mpp_pcbrele(mpp))
+		MPP_UNLOCK(mpp);
+	return;
+}
+
+int
+mpp_pcbrele(struct mppcb *mpp)
+{
+	KASSERT(mpp->mpp_refcount > 0, ("%s: refcount 0", __func__));
+	MPP_LOCK_ASSERT(mpp);
+	if (refcount_release(&mpp->mpp_refcount) == 0)
+		return (0);
+
+	SDT_PROBE1(mptcp, session, mpp_pcbrele, mppcb_release, mpp);
+	printf("%s: %p\n", __func__, mpp);
+
+	/* released last reference to mpp */
+	crfree(mpp->mpp_cred);
+
+	MPP_UNLOCK(mpp);
+	MPP_LOCK_DESTROY(mpp); // XXX: to change
+	uma_zfree(V_mppcb_zone, mpp);
+
+    return (1);
+}
+
+void
+mpp_init(void) {
+	V_mppcb_zone = uma_zcreate("mppcb", sizeof(struct mppcb),
+					NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
+	uma_zone_set_max(V_mppcb_zone, maxsockets);
+}
diff -r 1d1c4c997b66 sys/netinet/mptcp_pcb.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_pcb.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,88 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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.
+ */
+
+#ifndef MPTCP_PCB_H_
+#define MPTCP_PCB_H_
+
+#define mpptompcb(mpp)	(mpp)->mpp_mpcb
+
+/* XXXNJW Should define an mpp conninfo struct for fibnum, ports */
+
+/*
+ * Multi-path Protocol Control Block.
+ *
+ * Replacement of the INET PCB (inpcb), used in standard TCP/SOCK_STREAM
+ * sockets. The inpcb is still used on each of the subflow sockets.
+ */
+struct mppcb {
+	struct socket *mpp_socket;	/* pointer to socket of master subflow */
+	struct mpcb *mpp_mpcb;		 /* per-protocol control block (mptcp only) */
+    struct mtx mppcb_mutex;	 /* mutex for the protocol control block */
+    struct	ucred	*mpp_cred;	/* cache of socket cred */
+    u_int16_t mpp_flags;      /* Connection flags (TIMEWAIT, DROPPED)*/
+	uint8_t	mpp_status;		 /* Connection status _UNUSED_ */
+	u_int mpp_refcount;          /* refcount */
+
+	/* To move to a 'conninfo'-type struct */
+    int mpp_fibnum;
+    in_port_t mpp_lport;
+    in_port_t mpp_fport;
+    uint32_t mpp_laddr;
+    uint32_t mpp_faddr;
+};
+
+
+/* MP Flags (mp_flags) */
+#define MPP_TIMEWAIT		0x0001	/* MP state machine time-wait */
+#define MPP_DROPPED		0x0002	/* protocol dropped */
+#define MPP_SOCKREF		0X0004	/* Strong socket reference */
+
+/* Mutex for the MPCB */
+#define MPP_LOCK_INIT(mpp) mtx_init(&mpp->mppcb_mutex, "mppcb", NULL, MTX_DEF)
+#define MPP_LOCK_DESTROY(mpp) mtx_destroy(&mpp->mppcb_mutex)
+#define MPP_LOCK(mpp)      mtx_lock(&mpp->mppcb_mutex)
+#define MPP_LOCKED(mpp)   mtx_owned(&(mpp)->mppcb_mutex)
+#define MPP_UNLOCK(mpp)	   mtx_unlock(&mpp->mppcb_mutex)
+#define	MPP_LOCK_ASSERT(mpp) mtx_assert(&mpp->mppcb_mutex, MA_OWNED)
+
+int mpp_pcballoc(struct socket *);
+int mpp_getsockaddr(struct socket *so, struct sockaddr **nam);
+int mpp_getpeeraddr(struct socket *so, struct sockaddr **nam);
+int	mpp_pcbrele(struct mppcb *mpp);
+void mpp_pcbrele_unlocked(struct mppcb *mpp);
+void mpp_pcbdetach(struct mppcb *);
+void mpp_pcbdrop(struct mppcb *);
+void mpp_pcbfree(struct mppcb *);
+void mpp_pcbref(struct mppcb *mpp);
+
+#endif /* MPTCP_PCB_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_reass.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_reass.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,262 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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/protosw.h>
+#include <sys/refcount.h>
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/protosw.h>
+#include <sys/random.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_usrreq.h>
+#include <netinet/tcp_fsm.h>
+#include <netinet/tcp_seq.h>
+#include <netinet/tcp_syncache.h>
+
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_timer.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
+
+#include <machine/in_cksum.h>
+#include <machine/stdarg.h>
+
+
+
+int
+mp_reass(struct mpcb *mp, struct mbuf *m)
+{
+	struct mbuf *mq, *mpre;
+	struct socket *so = mp->mp_mppcb->mpp_socket;
+	int flags = 0, wakeup = 0, todrop;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	M_ASSERTPKTHDR(m);
+    KASSERT(m_tag_locate(m, PACKET_COOKIE_MPTCP, PACKET_TAG_DSN, NULL) != NULL,
+        ("%s: no dsn tag on segment\n", __func__));
+
+//	printf("%s: packetlen %d\n", __func__, m->m_pkthdr.len);
+//    printf("%s: ds_rcv_nxt: %u mdsn %u\n", __func__,
+//        (uint32_t) mp->ds_rcv_nxt, (uint32_t) M_MPTCPDSN(m));
+//    printf("%s: ds_rcv_nxt: %ju mdsn %ju\n", __func__,
+//		mp->ds_rcv_nxt, M_MPTCPDSN(m));
+    if (M_MPTCPDSNFLAGS(m) & MP_DFIN)
+		printf("%s: inserting dfin seg %u ds_rcv_nxt: %u\n",__func__,
+		    (uint32_t) mp->ds_rcv_nxt, (uint32_t) M_MPTCPDSN(m));
+
+    /* Trim overlapping at data level, or drop if duplicate */
+    todrop = mp->ds_rcv_nxt - M_MPTCPDSN(m);
+    if (todrop > 0) {
+//    	printf("%s: dup segment %u len %d todrop %d \n", __func__,
+//    		(uint32_t) M_MPTCPDSN(m), m->m_pkthdr.len, todrop);
+    	/* Partially duplicated segment. Trim until
+		 * we reach the new data. Otherwise a complete
+		 * duplicate that can be freed. goto present
+		 * to read in any queued data. */
+    	if (todrop < m->m_pkthdr.len) {
+			M_MPTCPDSN(m) += todrop;
+			m_adj(m, todrop);
+		} else {
+			m_freem(m);
+			if (mp->mp_segq)
+			    goto present;
+			else
+				return (0);
+		}
+    }
+
+	/*
+	 * Find a segment which begins after this one does.
+     * XXX: adjust for dealing with DSNs
+	 */
+	mpre = NULL;
+	for (mq = mp->mp_segq; mq != NULL; mq = mq->m_nextpkt) {
+//		printf("%s: mqdsn %lu mdsn %lu\n", __func__,
+//		    M_MPTCPDSN(mq), M_MPTCPDSN(m));
+		if (DSEQ_GT((uint64_t)M_MPTCPDSN(mq), (uint64_t)M_MPTCPDSN(m)))
+			break;
+		mpre = mq;
+//		printf("%s: mpre set, dsn %u\n", __func__, (uint32_t) M_MPTCPDSN(mq));
+	}
+
+	/*
+	 * If there is a preceding segment, it may provide some of
+	 * our data already.  If so, drop the data from the incoming
+	 * segment.  If it provides all of our data, drop us.
+     *
+     * XXX: in this case dealing with DSNs rather than TCP SEQs.
+     * So previous_dsn + previous_len compared with the DSN on
+     * the passed in mbuf.
+     *
+     * After adjustment we change the DSN on the mbuf tag. Note
+     * that the
+	 */
+	if (mpre != NULL) {
+		int i;
+
+		/* conversion to int (in i) handles seq wraparound */
+		i = M_MPTCPDSN(mpre) + mpre->m_pkthdr.len - M_MPTCPDSN(m);
+		if (i > 0) {
+			if (i >= m->m_pkthdr.len) {
+				m_freem(m);
+				/*
+				 * Try to present any queued data
+				 * at the left window edge to the user.
+				 * This is needed after the 3-WHS
+				 * completes.
+				 */
+				goto present;	/* ??? */
+			}
+			m_adj(m, i);
+			M_MPTCPDSN(m) += i;
+		}
+	}
+
+	/*
+	 * While we overlap succeeding segments trim them or,
+	 * if they are completely covered, dequeue them.
+	 */
+	while (mq) {
+		struct mbuf *nq;
+		int i;
+
+		i = (M_MPTCPDSN(m) + m->m_pkthdr.len) - M_MPTCPDSN(mq);
+		if (i <= 0)
+			break;
+		if (i < mq->m_pkthdr.len) {
+			M_MPTCPDSN(mq) += i;
+			m_adj(mq, i);
+			mp->mp_segqlen -= i;
+			break;
+		}
+
+		nq = mq->m_nextpkt;
+		mp->mp_segqlen -= mq->m_pkthdr.len;
+		m_freem(mq);
+		if (mpre)
+			mpre->m_nextpkt = nq;
+		else
+			mp->mp_segq = nq;
+		mq = nq;
+	}
+
+	/*
+	 * Insert the new (data-level) segment queue entry into place.
+	 */
+	if (mpre) {
+		m->m_nextpkt = mpre->m_nextpkt;
+		mpre->m_nextpkt = m;
+	} else {
+		mq = mp->mp_segq;
+		mp->mp_segq = m;
+		m->m_nextpkt = mq;
+	}
+	mp->mp_segqlen += m->m_pkthdr.len;
+
+present:
+
+    /* XXXNJW: check to see if first segment is in order.
+     * if so, schedule mp_input, which will append the
+     * data to the buffer. */
+	mq = mp->mp_segq;
+//    printf("%s: present, got %ju, rcv_nxt %ju\n", __func__,
+//		M_MPTCPDSN(mq), mp->ds_rcv_nxt);
+
+	SOCKBUF_LOCK(&so->so_rcv);
+	while ((mq = mp->mp_segq) != NULL &&
+		M_MPTCPDSN(mq) == mp->ds_rcv_nxt) {
+
+		mp->mp_segq = mq->m_nextpkt;
+		mp->ds_rcv_nxt += mq->m_pkthdr.len;
+		mp->mp_segqlen -= mq->m_pkthdr.len;
+
+//		printf("%s: rcv_nst now %ju\n", __func__, mp->ds_rcv_nxt);
+
+		/* XXXNJW: temp way to handle receipt of DFIN. Need to +1
+		 * ds_rcv_nxt as generally it is increased by segment length
+		 * rather than the dss_len (also currently the dss len isn't
+		 * included in the mtag) */
+		flags = M_MPTCPDSNFLAGS(mq) & MP_DFIN;
+
+		if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
+			m_freem(mq);
+		else {
+			mq->m_nextpkt = NULL;
+			sbappendstream_locked(&so->so_rcv, mq, 0);
+			wakeup = 1;
+		}
+	}
+
+	if (wakeup) {
+		mp->mp_flags |= MPF_ACKNOW;
+		sorwakeup_locked(so);
+	} else
+		SOCKBUF_UNLOCK(&so->so_rcv);
+
+	return (flags);
+
+}
diff -r 1d1c4c997b66 sys/netinet/mptcp_sched.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_sched.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,118 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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.
+ */
+
+#ifndef MPTCP_SCHED_H_
+#define MPTCP_SCHED_H_
+
+#include <netinet/tcp.h>
+#include <netinet/mptcp_var.h>
+
+/* Global scheduler vars. */
+extern STAILQ_HEAD(sched_head, sched_algo) sched_list;
+extern struct sched_disc roundrobin_sched_algo;
+
+/* Per-netstack bits. */
+VNET_DECLARE(struct sched_algo *, default_sched_ptr);
+#define	V_default_sched_ptr VNET(default_sched_ptr)
+
+/* Define the new net.inet.tcp.mptcp.sched sysctl tree. */
+SYSCTL_DECL(_net_inet_tcp_mptcp_sched);
+
+/* Scheduler housekeeping functions. */
+int	sched_register_algo(struct sched_algo *add_sched_algo);
+int	sched_deregister_algo(struct sched_algo *remove_sched_algo);
+
+/*
+ * Struct holding data to be passed from the tcpcb to the scheduling algorithm
+ */
+struct sched_var {
+	void        *sched_data; /* Per-connection private sched algorithm data. */
+	uint32_t    flags; /* Flags for sched_var (see below) */
+	struct mpcb *mp;
+	struct tcpcb *tcp; /* TCP control block of calling subflow */
+	struct tcpcb *sched_tp; /* The pcb selected by algorithm */
+};
+
+/* sched_var flags */
+// Any flags relevant to _all_ schedulers should be defined here
+
+/*
+ * Structure to hold data and function pointers that together represent a
+ * packet scheduling discipline.
+ */
+struct sched_algo {
+	char	name[MPTCP_SA_NAME_MAX];
+
+	/* Init global module state on kldload. */
+	int	(*mod_init)(void);
+
+	/* Cleanup global module state on kldunload. */
+	int	(*mod_destroy)(void);
+
+	/* Init scheduler state for a new control block. */
+	int	(*cb_init)(struct sched_var *sched);
+
+	/* Cleanup scheduler state for a terminating control block. */
+	void	(*cb_destroy)(struct sched_var *sv);
+
+	/* Init variables for a newly established connection. */
+	void	(*conn_init)(struct sched_var *sv);
+
+	/* Called on writing new data to socket buffer (tcp_usr_send) */
+	void	(*sched_usr_send)(struct sched_var *sv, uint16_t type);
+
+	/* Called on execution of scheduler task */
+	void	(*sched_task)(struct sched_var *sv, uint16_t type);
+
+	STAILQ_ENTRY (sched_algo) entries;
+};
+
+/* Macro to obtain the sched algo's struct ptr. */
+#define	SCHED_ALGO(mp)	((mp)->sched_algo)
+
+/* Macro to obtain the sched algo's data ptr. */
+#define	SCHED_DATA(mp)	((mp)->sched_v->sched_data)
+
+/* Macro to obtain the system default Scheduler algo's struct ptr. */
+#define	SCHED_DEFAULT() V_default_sched_ptr
+
+extern struct rwlock sched_list_lock;
+#define	SCHED_LIST_LOCK_INIT()	    rw_init(&sched_list_lock, "sched_list")
+#define	SCHED_LIST_LOCK_DESTROY()	rw_destroy(&sched_list_lock)
+#define	SCHED_LIST_RLOCK()		    rw_rlock(&sched_list_lock)
+#define	SCHED_LIST_RUNLOCK()	    rw_runlock(&sched_list_lock)
+#define	SCHED_LIST_WLOCK()		    rw_wlock(&sched_list_lock)
+#define	SCHED_LIST_WUNLOCK()	    rw_wunlock(&sched_list_lock)
+#define	SCHED_LIST_LOCK_ASSERT()	rw_assert(&sched_list_lock, RA_LOCKED)
+
+#endif /* MPTCP_SCHED_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_subr.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_subr.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,3758 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD
+ * Foundation and 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/protosw.h>
+#include <sys/refcount.h>
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/protosw.h>
+#include <sys/random.h>
+
+/* for SCTP auth functions */
+#include <crypto/sha1.h>
+
+/* for checking interface status */
+#include <net/if.h>
+
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_usrreq.h>
+#include <netinet/tcp_fsm.h>
+#include <netinet/tcp_seq.h>
+#include <netinet/tcp_syncache.h>
+
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_timer.h>
+#include <netinet/mptcp_dtrace_declare.h>
+
+/* for SCTP auth functions */
+#include <netinet/sctp_pcb.h>
+#include <netinet/sctp_auth.h>
+#include <netinet/sctp_constants.h>
+
+#include <machine/in_cksum.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, max_subflows) = 8;
+#define	V_mptcp_max_subflows	VNET(mptcp_max_subflows);
+SYSCTL_INT(_net_inet_tcp_mptcp, OID_AUTO, max_subflows,
+	CTLFLAG_RW, &VNET_NAME(max_subflows), 0,
+    "Maximum number of subflows per Multipath TCP Connection");
+
+VNET_DEFINE(int, single_packet_maps) = 1;
+#define	V_mptcp_single_packet_maps	VNET(mptcp_single_packet_maps);
+SYSCTL_INT(_net_inet_tcp_mptcp, OID_AUTO, single_packet_maps,
+    CTLFLAG_RW, &VNET_NAME(single_packet_maps), 0,
+    "DSN Maps cover a single packet only");
+
+VNET_DECLARE(int, nomptimewait);
+#define	V_nomptimewait  VNET(nomptimewait)
+VNET_DEFINE(int, nomptimewait) = 1;
+SYSCTL_INT(_net_inet_tcp_mptcp, OID_AUTO, nomptimewait,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(nomptimewait), 0,
+    "Do not use the timeout timer when entering MPTCP TIME WAIT");
+
+VNET_DECLARE(int, mptimerlength);
+#define	V_mptimerlength  VNET(mptimerlength)
+VNET_DEFINE(int, mptimerlength) = 0;
+SYSCTL_INT(_net_inet_tcp_mptcp, OID_AUTO, mptimerlength,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(mptimerlength), 0,
+    "MPTCP timeout interval length");
+
+
+static VNET_DEFINE(uma_zone_t, mpcb_zone);
+#define	V_mpcb_zone			VNET(mpcb_zone)
+
+//static VNET_DEFINE(uma_zone_t, mpsopt_zone);
+//#define V_mpsopt_zone VNET(mpsopt_zone)
+
+#define mp_timeout() V_mptimerlength ? V_mptimerlength*hz : MPTCPTV_TIMEOUT
+
+#define M_MPTCPDSN(m) \
+    (((struct dsn_tag *)(m_tag_locate(m, PACKET_COOKIE_MPTCP, \
+    PACKET_TAG_DSN, NULL)))->dsn)
+
+#define M_MPTCPDSNFLAGS(m) \
+    (((struct dsn_tag *)(m_tag_locate(m, PACKET_COOKIE_MPTCP, \
+    PACKET_TAG_DSN, NULL)))->dss_flags)
+
+MALLOC_DEFINE(M_REASSLISTHEAD, "reasshead", "Head of reass queue list");
+
+struct debug_class {
+	char class[32];
+	int verbosity;
+	int loglevelexclusive;
+};
+
+struct mpcb_mem {
+	struct	mpcb		mpcb;
+};
+
+uint32_t mp_active_debug_classes;
+
+struct debug_class debug_classes[] = {
+		{.class = "MPSESSION"},
+		{.class = "DSMAP"},
+		{.class = "SBSTATUS"},
+		{.class = "REASS"}
+};
+
+#define	N_DEBUGCLASSES (sizeof(debug_classes)/sizeof(struct debug_class))
+
+MALLOC_DEFINE(M_SFHANDLE, "sfhandle",
+     "Handle containing subflow information: inp, tcp, gso");
+MALLOC_DEFINE(M_MPTOKINFO, "mptokeninfo",
+     "MP sessions and their (local, remote) tokens");
+MALLOC_DEFINE(M_MPTIMERS, "mptimers",
+     "Timer callouts for MP sessions");
+MALLOC_DEFINE(M_MPSOPT, "mpsopt",
+     "Record of socket options on MP connection");
+
+static int mp_subflow_detached(struct mpcb *mp, int count);
+static int mp_close_subflow(struct mpcb *mp);
+static int mp_detached_last_subflow(struct mpcb *mp);
+static void mp_process_subflow_event(struct mpcb *mp, struct tcpcb *tp);
+static void mp_set_default_address(struct mpcb *mp, struct inpcb *inp);
+static void mp_set_connection_info(struct mpcb *mp, struct tcpcb *tp);
+static void mp_sf_connected(struct mpcb *mp, struct tcpcb *tp);
+static int  mp_join_learned(struct mpcb *mp, struct socket *so);
+static int  mp_join_from_advertised(struct mpcb *mp, struct socket *so);
+static int mp_join_do_connect(struct socket *so, void* laddr, void* faddr,
+	u_int16_t lport, u_int16_t fport);
+static void	mp_update_recwin(struct mpcb *mp);
+static int  mp_do_output(struct socket *so, struct mpcb *mp,
+	struct sf_handle *sf, int flags);
+static void mp_input(struct mpcb *mp, struct socket *so);
+static void mp_standard_input_ack(struct mpcb *mp, struct socket *so);
+static int mp_do_segment(struct mbuf *m, struct tcphdr *th, struct socket *so,
+    struct mpcb *mp, int tlen);
+static void mp_dooptions(struct tcpopt *to, u_char *cp, int cnt);
+static void mp_twstart(struct mpcb *mp);
+
+static struct sf_handle * mp_get_subflow(struct mpcb *mp);
+static struct ds_map* mp_get_map(struct mpcb *mp, int length);
+static uint64_t mp_dsn32to64(uint64_t ds_val, uint64_t dseq);
+static uint64_t mp_ack32to64(uint64_t dack_val, uint64_t dseq);
+
+static void mp_reass_flush(struct mpcb *mp);
+
+
+/* XXXNJW: TEMP PATH MANAGEMENT */
+/* keeping track of addresses added via sysctl:
+ * net.inet.tcp.mptcp.mp_addresses. Though we start at '1' as the 'master'
+ * address is included by default.
+ */
+int mp_address_count = 1;
+struct sockaddr_storage mp_usable_addresses[MAX_ADDRS];
+
+/* Initialise extern variables that store addresses */
+struct sockaddr_storage mp_path_manager[MAX_ADDRS];
+/* --- */
+
+struct mp_sessions mp_tokinfo_list;
+
+#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, loglevelexclusive;
+
+	effective_verbosity = 0;
+
+	if (log_class & mp_active_debug_classes) {
+	// calculate shift index (index of class into array)
+		i = 0;
+		loglevelexclusive = 0;
+		while(log_class >= 1) {
+			if (log_class & 0x1) {
+				effective_verbosity = max(debug_classes[i].verbosity, effective_verbosity);
+				loglevelexclusive += debug_classes[i].loglevelexclusive;
+			}
+			log_class >>= 1;
+			i++;
+		}
+
+		if	((loglevelexclusive && msg_verbosity == effective_verbosity) ||
+				(!loglevelexclusive && msg_verbosity <= effective_verbosity)) {
+			va_start(ap, fmt);
+			vprintf(fmt, ap);
+			va_end(ap);
+		}
+	}
+}
+
+/*
+ * Sysctl handler to show and change addito subflow addresses
+ */
+static int
+mp_addresses(SYSCTL_HANDLER_ARGS)
+{
+	char *straddr;
+	struct sbuf *s;
+	void *addr;
+	char inet_buf[64];
+	int error, i, ret;
+
+	error = 0;
+
+	if (req->newptr == NULL) {
+		s = sbuf_new_for_sysctl(NULL, NULL, 300, req);
+		if (s == NULL)
+			return (ENOMEM);
+
+		/* Start at i = 1 as the 'master' address is always included
+		 * by default */
+		for (i = 1; i < mp_address_count; i++) {
+			switch (mp_usable_addresses[i].ss_family) {
+				case AF_INET:
+					addr = &((struct sockaddr_in *)
+					    &mp_usable_addresses[i])->sin_addr;
+					break;
+				case AF_INET6:
+					addr = &((struct sockaddr_in6 *)
+					    &mp_usable_addresses[i])->sin6_addr;
+					break;
+				default:
+					return (EINVAL);
+			}
+
+			inet_ntop(mp_usable_addresses[i].ss_family, addr, inet_buf,
+			    mp_usable_addresses[i].ss_len);
+			sbuf_cat(s, inet_buf);
+			if (i < (mp_address_count - 1))
+				sbuf_cat(s, " ");
+		}
+
+		error = sbuf_finish(s);
+		sbuf_delete(s);
+	} else {
+		i = 1;
+		while ((straddr = strsep((char **)&req->newptr, " "))
+			!= NULL && i < MAX_ADDRS && !error) {
+			ret = inet_pton(AF_INET6, straddr,
+			    &((struct sockaddr_in6 *)&mp_usable_addresses[i])->sin6_addr);
+			if (ret == 1) {
+				mp_usable_addresses[i].ss_family = AF_INET6;
+				mp_usable_addresses[i].ss_len = sizeof(struct sockaddr_in6);
+			} else if (ret == 0) {
+				ret = inet_pton(AF_INET, straddr,
+				&((struct sockaddr_in *)&mp_usable_addresses[i])->sin_addr);
+				if (ret == 1) {
+					mp_usable_addresses[i].ss_family = AF_INET;
+					mp_usable_addresses[i].ss_len = sizeof(struct sockaddr_in);
+				}
+			}
+
+			/* If there was no valid address, ret will be < 1. */
+			if (ret < 1) {
+				char *zero = "0";
+				mp_address_count = 1;
+				if (strcmp(straddr, zero) == 0)
+					return(0);
+				error = EINVAL;
+			}
+			i++;
+		}
+
+		if (!error) {
+			if (i == mp_address_count)
+				error = E2BIG; // most likely addresses exceeded MAX_ADDRS
+			else
+				mp_address_count = i;
+		}
+	}
+
+	return (error);
+}
+
+/*
+ * Sysctl handler for setting debug output levels for mp
+ */
+static int
+mp_debug_sysctl_handler(SYSCTL_HANDLER_ARGS)
+{
+	struct sbuf *s;
+	int error, i;
+
+	error = 0;
+
+	if (req->newptr == NULL) {
+		s = sbuf_new_for_sysctl(NULL, NULL, 300, req);
+		if (s == NULL)
+			return (ENOMEM);
+
+		for(i = 0; i < N_DEBUGCLASSES; i++) {
+				if (i > 0)
+					sbuf_putc(s, ',');
+				sbuf_printf(s, "%s:%c%d", debug_classes[i].class,
+					debug_classes[i].loglevelexclusive ? '=' : '*',
+					debug_classes[i].verbosity);
+		}
+
+		error = sbuf_finish(s);
+		sbuf_delete(s);
+	} else {
+		char *class, *pair;
+
+		while ((pair = strsep(((char **)&req->newptr), ",")) != NULL) {
+			class = strsep(&pair, ":");
+			for(i = 0; i < N_DEBUGCLASSES; i++) {
+				if (strcmp(class, debug_classes[i].class) == 0
+					|| strcmp(class, "ALL") == 0) {
+					if (pair[0] == '=') {
+						debug_classes[i].loglevelexclusive = 1;
+						pair++;
+					} else {
+						if (pair[0] == '*')
+							pair++;
+						debug_classes[i].loglevelexclusive = 0;
+					}
+					debug_classes[i].verbosity = strtol(pair, NULL, 10);
+					if (debug_classes[i].verbosity == 0)
+						mp_active_debug_classes &= ~(1<<i);
+					else
+						mp_active_debug_classes |= 1<<i;
+				}
+			}
+		}
+	}
+
+	return (error);
+}
+
+/*
+ * Parse MP options and place in tcpopt->to_mopts.
+ */
+static void
+mp_dooptions(struct tcpopt *to, u_char *cp, int cnt)
+{
+	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)
+			break;
+		if (opt == TCPOPT_NOP)
+			optlen = 1;
+		else {
+			if (cnt < 2)
+				break;
+			optlen = cp[1];
+			if (optlen < 2 || optlen > cnt)
+				break;
+		}
+		switch (opt) {
+		case TCPOPT_MPTCP:
+			bcopy((char *)cp + 2,
+					(char *)&subtype, sizeof(subtype));
+			if (optlen > MAX_MP_OPLEN)
+				continue;
+			mp_dosubtypes(to, subtype, cp, opt, optlen, 0);
+			break;
+		default:
+			continue;
+		}
+	}
+}
+
+void
+mp_dosubtypes(struct tcpopt *to, uint8_t subtype, u_char *cp, int opt,
+	int optlen, int flags) {
+	int byte_offset;
+	struct mp_capable mcap;
+	struct mp_join mjoin;
+
+	subtype = subtype >> 4;
+
+	switch(subtype)
+	{
+	case MPTCP_SUBTYPE_MP_CAPABLE:
+	{
+		/* XXXNJW:
+		 * have to check that the 's' bit is set
+		 * also should check for the dss csum bit (for now just ignore it...)
+		 */
+		if (optlen != MPTCP_SUBLEN_MP_CAPABLE_SYN &&
+				optlen != MPTCP_SUBLEN_MP_CAPABLE_ACK) {
+			break;
+		}
+
+		/* indicate we have mptcp options present */
+		to->to_flags |= TOF_MPTCP;
+		to->to_mopts.mpo_flags |= MPOF_MP_CAPABLE;
+
+		if ((flags & TO_SYN) &&
+				(optlen == MPTCP_SUBLEN_MP_CAPABLE_SYN)) {
+
+				bcopy((char *)cp,(char *)&mcap,
+						sizeof(struct mp_capable));
+
+				/* using checksums? */
+				if (mcap.flags & USE_CSUM) {
+					to->to_mopts.mpo_flags |= MPOF_USE_CSUM;
+				}
+
+				/* need to save the incoming key */
+				bcopy((char *)cp + MP_REMOTE_KEY_OFFSET,
+						(char *)&to->to_mopts.remote_key,
+						sizeof(to->to_mopts.remote_key));
+		}
+
+		if (optlen == MPTCP_SUBLEN_MP_CAPABLE_ACK) {
+			/* do some validation here */
+
+			/* XXXNJW: put in a flag that tells a passive opener to go
+			 * directly into MPTCP mode (rather than wait around in
+			 * INFINITE_MAP mode for a couple of packets. This is read
+			 * during syncache_expand, and causes mp_init_established to
+			 * be called (while we already hole the MP_LOCK). */
+			to->to_mopts.mpo_flags |= MPOF_CAPABLE_ACK;
+		}
+		break;
+	}
+	case MPTCP_SUBTYPE_DSS:
+	{
+			byte_offset = 0;
+			uint8_t	dss_flags;
+
+			to->to_flags |= TOF_MPTCP;
+			to->to_mopts.mpo_flags |= MPOF_DSS;
+			bcopy((char *)cp + MP_DSS_FLAGS_OFFSET,(char *)&dss_flags,
+							sizeof(uint8_t));
+
+			/* Data-FIN present */
+			if (dss_flags & FIN_PRESENT)
+				to->to_mopts.mpo_flags |= MPOF_DATA_FIN;
+
+			/* Data-ACK present */
+			if (dss_flags & ACK_PRESENT) {
+				// set flag that we have an ACK
+				to->to_mopts.mpo_flags |= MPOF_DATA_ACK;
+
+				/* Is 64-bit Data ACK present */
+				if (dss_flags & ACK_64_PRESENT) {
+					to->to_mopts.mpo_flags |= MPOF_ACK64;
+					// grabbing whole 64-bits
+					bcopy((char *)cp + MP_DATA_ACK64_OFFSET,
+						(char *)&to->to_mopts.data_ack_num,
+						sizeof(to->to_mopts.data_ack_num));
+					to->to_mopts.data_ack_num =
+							be64toh(to->to_mopts.data_ack_num);
+					// offset the presence of 64-bit ACK
+					byte_offset += 8;
+				} else {
+					uint32_t short_data_ack;
+					bcopy((char *)cp + MP_DATA_ACK_OFFSET,
+						(char *)&short_data_ack,
+						sizeof(short_data_ack));
+					to->to_mopts.data_ack_num = ntohl(short_data_ack);
+					byte_offset += 4; // offset the presence of 32-bit ACK
+				}
+			}
+
+			/* DSN, SSN, length and csum present */
+			if (dss_flags & MAP_PRESENT) {
+				/* flag the new DSN Mapping for this subflow */
+				to->to_mopts.mpo_flags |= MPOF_DSN_MAP;
+
+				/* Is DSN 8 octets */
+				if (dss_flags & DSN_64) {
+					bcopy((char *)cp + MP_DSN_OFFSET + byte_offset,
+						(char *)&to->to_mopts.data_seq_num,
+						sizeof(to->to_mopts.data_seq_num));
+					to->to_mopts.data_seq_num =
+					    be64toh(to->to_mopts.data_seq_num);
+					byte_offset += 4;
+					to->to_mopts.mpo_flags |= MPOF_DSN64;
+				} else {
+					uint32_t short_dsn;
+					bcopy((char *)cp + MP_DSN_OFFSET + byte_offset,
+						(char *)&short_dsn,
+						sizeof(short_dsn));
+					to->to_mopts.data_seq_num = (uint64_t) ntohl(short_dsn);
+				}
+
+				/* subflow sequence number */
+				bcopy((char *)cp + MP_SUB_SEQN_OFFSET + byte_offset,
+					(char *)&to->to_mopts.sub_seq_num,
+					sizeof(to->to_mopts.sub_seq_num));
+				to->to_mopts.sub_seq_num = ntohl(to->to_mopts.sub_seq_num);
+
+				/* Data-level length */
+				bcopy((char *)cp + MP_DATA_LEN_OFFSET + byte_offset,
+					(char *)&to->to_mopts.dss_data_len,
+					sizeof(to->to_mopts.dss_data_len));
+				to->to_mopts.dss_data_len = ntohs(to->to_mopts.dss_data_len);
+
+				/*
+				 * XXXNJW: need to check if using csums
+				 * should do this by checking the length of the option
+				 * at the start and keeping track along the way....
+				 */
+
+				 /* checksum */
+//					bcopy((char *)cp + MP_CSUM_OFFSET + byte_offset,
+//						(char *)&to->to_mopts.dss_csum,
+//						sizeof(to->to_mopts.dss_csum));
+//					to->to_mopts.dss_csum = ntohs(to->to_mopts.dss_csum);
+			}
+			break;
+	}
+	case MPTCP_SUBTYPE_MP_JOIN:
+	{
+		if (optlen != MPTCP_SUBLEN_MP_JOIN_SYN &&
+			optlen != MPTCP_SUBLEN_MP_JOIN_SYNACK &&
+			optlen != MPTCP_SUBLEN_MP_JOIN_ACK) {
+			break;
+		}
+
+		to->to_flags |= TOF_MPTCP;
+		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.mpo_flags |= MPOF_JOIN_SYN;
+			break;
+		case MPTCP_SUBLEN_MP_JOIN_SYNACK:
+			to->to_flags |= TOF_MPTCP;
+
+			/* copy option 'header' into struct */
+			bcopy((char *)cp,(char *)&mjoin,
+				sizeof(struct mp_join));
+			/* is this a backup path */
+			if (mjoin.sub_flags & IS_BACKUP) {
+				to->to_mopts.mpo_flags |= MPOF_BACKUP_PATH;
+			}
+			/* save remote truncate MAC */
+			bcopy((char *)cp + MP_SND_MAC_OFFSET,
+			    (char *)&to->to_mopts.snd_trc_mac,
+			    sizeof(to->to_mopts.snd_trc_mac));
+
+			/* save the senders random number */
+			bcopy((char *)cp + MP_SND_RND_SYNACK_OFFSET,
+			    (char *)&to->to_mopts.snd_rnd,
+			    sizeof(to->to_mopts.snd_rnd));
+
+			to->to_mopts.mpo_flags |= MPOF_JOIN_SYNACK;
+			break;
+		case MPTCP_SUBLEN_MP_JOIN_ACK:
+			to->to_flags |= TOF_MPTCP;
+
+			/* just take a pointer to the hmac. We can copy the bytes in
+			 * syncache_expand_subflow as the memory will still be valid */
+			to->to_mopts.snd_mac = (char *)cp + MP_SND_MAC_OFFSET;
+
+			to->to_mopts.mpo_flags |= MPOF_JOIN_ACK;
+			to->to_mopts.mpo_flags |= MPOF_NEED_ACK;
+			break;
+		}
+		break;
+	}
+	case MPTCP_SUBTYPE_ADD_ADDR:
+	{
+		/* not using the port number field for now */
+		if (optlen != MPTCP_SUBLEN_ADD_ADDRV4 &&
+		    optlen != MPTCP_SUBLEN_ADD_ADDRV6)
+				break;
+
+		void * addr = NULL;
+		int addr_len = 0;
+		to->to_flags |= TOF_MPTCP;
+
+		switch (optlen)
+		{
+		case MPTCP_SUBLEN_ADD_ADDRV4:
+			to->to_mopts.mpo_flags |= MPOF_ADD_ADDR_V4;
+			addr = &((struct sockaddr_in *)
+					&to->to_mopts.new_addr)->sin_addr.s_addr;
+			addr_len = 4;
+			/* Copy address ID */
+			break;
+		case MPTCP_SUBLEN_ADD_ADDRV6:
+			to->to_mopts.mpo_flags |= MPOF_ADD_ADDR_V6;
+			/* Copy INET address */
+			addr = &((struct sockaddr_in6 *)
+					&to->to_mopts.new_addr)->sin6_addr.__u6_addr;
+			addr_len = 16;
+			break;
+		}
+		/* Copy INET address */
+		bcopy((char *)cp + MP_ADD_ADDR_OFFSET, addr,
+		    addr_len);
+
+		char buf[128];
+		inet_ntop(AF_INET, &(((struct in_addr *) addr)->s_addr), buf, sizeof(buf));
+		mp_debug(MPSESSION, 4, 0, "option add_addr %s\n", buf);
+
+		/* Copy address ID */
+		bcopy((char *)cp + MP_ADDID_OFFSET,
+		    (char *)&to->to_mopts.addr_id,
+		    sizeof(to->to_mopts.addr_id));
+		break;
+	}
+	case MPTCP_SUBTYPE_REMOVE_ADDR:
+	{
+		to->to_flags |= TOF_MPTCP;
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_PRIO:
+	{
+		to->to_flags |= TOF_MPTCP;
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_FAIL:
+	{
+		to->to_flags |= TOF_MPTCP;
+		break;
+	}
+	case MPTCP_SUBTYPE_MP_FASTCLOSE:
+	{
+		if (optlen != MPTCP_SUBTYPELEN_MP_FASTCLOSE)
+			break;
+
+		to->to_flags |= TOF_MPTCP;
+		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 */
+}
+
+void
+mp_syncache_newmpcb(struct mpcb *mp, struct syncache *sc)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+//    mp->ds_idsn = sc->sc_ds_iss;
+//    mp->ds_idsr = sc->sc_ds_irs;
+
+    /* XXXNJW: if we know we are going to have an MP connection, should
+     * use the data-level accounting here. */
+    mp->ds_rcv_nxt = sc->sc_irs + 1;
+    mp->ds_snd_una = mp->ds_map_max = mp->ds_map_min = mp->ds_snd_nxt =
+        sc->sc_iss + 1;
+
+//    mp->local_key = sc->sc_local_key;
+//    mp->remote_key = sc->sc_remote_key;
+//    mp->local_token = sc->sc_mp_local_token;
+//    mp->remote_token = sc->sc_mp_remote_token;
+
+	/* Connected is set when a subflow is connected, whether MPTCP or
+	 * standard TCP */
+	mp->mp_connected = 0;
+
+	/* created through syncache, therefore came from a listening socket */
+//	mp->mp_passive = 1;
+
+    printf("%s: ds_idsn: %u ds_idsr: %u ds_rcv_nxt %u ds_map_max: %u ds_snd_una %u max %u\n",
+         __func__, (uint32_t) mp->ds_idsn, (uint32_t) mp->ds_idsr,
+         (uint32_t) mp->ds_rcv_nxt, (uint32_t) mp->ds_map_max,
+         (uint32_t) mp->ds_snd_una, (uint32_t) mp->ds_snd_max);
+}
+
+int
+mp_newmpcb(struct mppcb *mpp)
+{
+	struct mpcb_mem *mpm;
+	struct mpcb *mp;
+	int error = 0;
+
+	mpm = uma_zalloc(V_mpcb_zone, M_NOWAIT | M_ZERO);
+	if (mpm == NULL)
+		return (ENOBUFS);
+
+	mp = &mpm->mpcb;  // XXXNJW: don't really need to init via mpm
+
+	/* Pointer to the multipath protocol control block */
+	mp->mp_mppcb = mpp;
+
+	/* tp is set only when we have a LISTEN tp */
+	mp->m_cb_ref.mp = mp;
+	mp->m_cb_ref.inp = NULL;
+
+	/* Not an mptcp session to start. Mark as '1' once MP established */
+	mp->mp_session = 0;
+
+	/* Connected is set when a subflow is connected, whether MPTCP or
+	 * standard TCP */
+	mp->mp_connected = 0;
+
+	/* This will be changed to '1' if mp_usr_listen() is called. */
+	mp->mp_passive = 0;
+
+	/* Init list of subflow tpcbs */
+	TAILQ_INIT(&mp->sf_list);
+
+	/* List of socket options */
+	TAILQ_INIT(&mp->mp_sopt_list);
+
+	/* Per-mpcb task queue */
+//	mp->mp_tq = taskqueue_create("mp_taskq", M_NOWAIT,
+//		taskqueue_thread_enqueue, &mp->mp_tq);
+//	taskqueue_start_threads(&mp->mp_tq, 1, PI_NET, "mp %p taskq", &mp);
+
+	/* Init task queue handlers
+	 * XXXNJW: Some of these tasks are temporary and should be removed at some
+	 * point (e.g. datascheduler_task_handler used to kick tcp_output of
+	 * subflows while a proper packet scheduler is missing */
+
+	/*  */
+	TASK_INIT(&mp->subflow_event_task, 0, mp_subflow_event_task_handler,
+	    mp->mp_mppcb);
+
+	/* Perform rexmits when d-level RTO fires */
+//	TASK_INIT(&mp->rexmit_task, 0, mp_rexmit_task_handler, mp->mp_mppcb);
+
+    /* XXXNJW: temp, should get rid of this task at some point. */
+	TASK_INIT(&mp->subflow_close_task, 0, mp_close_subflow_task_handler,
+	    mp->mp_mppcb);
+
+	/* A subflow has removed itself from it's socket */
+	TASK_INIT(&mp->subflow_detached_task, 0, mp_subflow_detached_task_handler,
+	    mp->mp_mppcb);
+
+	/* Drop acked data, send new data */
+	TASK_INIT(&mp->mp_output_task, 0, mp_output_task_handler, mp->mp_mppcb);
+
+	/* Append to rcv buffer, process mp-level signals */
+	TASK_INIT(&mp->mp_input_task, 0, mp_input_task_handler, mp->mp_mppcb);
+
+	/* Asynchronous sending of MP_JOINs */
+	TASK_INIT(&mp->join_task, 0, mp_join_task_handler, mp->mp_mppcb);
+
+	/* Set the default scheduler for the connection */
+//	SCHED_ALGO(mp) = &sched_roundrobin;
+
+	/* mp-level timers */
+	mp->mp_timers = malloc(sizeof(struct mptcp_timer), M_MPTIMERS, M_NOWAIT);
+
+	callout_init(&mp->mp_timers->mpt_rexmt, CALLOUT_MPSAFE);
+	callout_init(&mp->mp_timers->mpt_timeout, CALLOUT_MPSAFE);
+
+	/* Zero the added address count - this increments when we receive
+	 * add_addr from the remote host */
+	mp->mp_added_address_count = 0;
+
+	/* Number of addresses available to this mp at creation time. Value may
+	 * differ from the global address count due to e.g. an interface not being
+	 * available at this time. */
+	mp->mp_conn_address_count = mp_address_count;
+
+	/*
+	 * Set the masks. We clear bits as ADD_ADDRs and JOINS are sent
+	 * for each of the subflows.
+	 */
+	uint32_t bitmask = 0xffffffff;
+	mp->mp_advaddr_mask = ~(bitmask << mp->mp_conn_address_count);
+
+	/* Don't want to advertise the 'primary' address so mask out now */
+	mp->mp_advaddr_mask &= ~1;
+	mp->mp_advjoin_mask = mp->mp_advaddr_mask;
+
+	/* Update the advertise address mask and count of addresses available to
+	 * the session */
+	mp_update_available_addresses(mp);
+
+	/* reference from the MPPCB */
+	mpp->mpp_mpcb = mp;
+
+	return (error);
+}
+
+struct mpcb *
+mp_drop(struct mpcb *mp, int error)
+{
+	struct socket *so = mp->mp_mppcb->mpp_socket;
+
+	printf("%s: mp %p\n", __func__, mp);
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	if (mp->mp_state >= MPS_M_ESTABLISHED)
+		mp->mp_state = MPS_M_CLOSED;
+
+	so->so_error = error;
+	return (mp_close(mp));
+}
+
+/*
+ * A hacky work around to create a new inpcb, tcpcb pair and ascociate them
+ * with the primary socket.
+ *
+ * XXXNJW: Need to revisit how the listen side of things works, so this code
+ * will change.
+ */
+int
+mp_bind_attach(struct socket *so, struct mpcb *mp,
+    struct sockaddr *nam, struct thread *td) {
+	struct inpcb *inp;
+    struct tcpcb *tp;
+    int error;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+    INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
+
+	/* inpcb and tcpcb required for listen */
+	error = in_pcballoc(so, &V_tcbinfo);
+	if (error) {
+		return (error);
+	}
+	inp = sotoinpcb(so);
+	inp->inp_vflag |= INP_IPV4;
+	tp = tcp_newtcpcb(inp);
+	if (tp == NULL) {
+		in_pcbdetach(inp);
+		in_pcbfree(inp);
+		return (ENOBUFS);
+	}
+	tp->t_state = TCPS_CLOSED;
+	tp->t_mpcb = mp;
+	tp->t_sf_flags |= SFF_LISTENTCPCB;
+	mp->m_cb_ref.inp = inp;
+	INP_WUNLOCK(inp);
+
+	error = tcp_usr_bind(so, nam, td);
+
+	/* Set the socket pcb back to the mppcb */
+	so->so_pcb = (caddr_t) mp->mp_mppcb;
+	so->so_snd.sb_flags &= ~SB_AUTOSIZE;
+	so->so_rcv.sb_flags &= ~SB_AUTOSIZE;;
+
+	return (error);
+}
+
+int
+mp_create_subflow_socket(struct socket *so, struct socket **sf_gso)
+{
+	int error = 0;
+
+	/* Allocate subflow gsock */
+	if ((error = mp_alloc_subflow_socket(so, sf_gso)))
+		goto out;
+	KASSERT(*sf_gso != NULL, ("mp_usr_connect: sf_gso NULL"));
+
+	/* XXXNJW: temp protosw struct that allows ghost sockets to use
+	 * tcp_usrreqs */
+	(*sf_gso)->so_proto = &sf_protosw;
+
+out:
+   return error;
+}
+
+/* XXXNJW: add some validation here, perhaps */
+int
+mp_connect_subflow(struct socket *so, struct sockaddr *nam, struct thread *td)
+{
+	return tcp_usr_connect(so, nam, td);
+}
+
+/* Only called for standard TCP connections to
+ * process ACKs. */
+static void
+mp_standard_input_ack(struct mpcb *mp, struct socket *so)
+{
+//	struct tcpcb *tp;
+	struct inpcb *inp;
+	struct sf_handle *sf = TAILQ_FIRST(&mp->sf_list);
+//	int need_output = 0;
+//    uint32_t acknum;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	if (sf->sf_flags & SFHS_MPENDED)
+		return;
+
+    inp = sotoinpcb(sf->sf_so);
+	KASSERT(inp != NULL, ("%s: inp == NULL mp %p ", __func__, mp));
+	INP_WLOCK(inp);
+	if (inp->inp_flags & (INP_DROPPED | INP_TIMEWAIT)) {
+		INP_WUNLOCK(inp);
+		return;
+	}
+	INP_WUNLOCK(inp);
+
+	mp_do_output(so, mp, sf, 0);
+
+    return;
+}
+
+/* mp_input does the "present" portion of mp_reass (i.e.
+ * sbappendstream). This includes calling wakeup on the
+ * recv buffer.
+ *
+ * Also process any data-level signalling, such as DFIN
+ * or DFIN-ACK (can we actually do this here?) *for now
+ * keep processing DFIN-ACK as a subflow event.
+ */
+static void
+mp_input(struct mpcb *mp, struct socket *so)
+{
+    int off, tlen = 0;
+    struct tcphdr *th = NULL;
+    struct ip *ip = NULL;
+    struct m_tag *mtag;
+    struct dsn_tag *dtag;
+    struct mbuf *m;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* process each segment independently */
+    while ((m = mp->mp_input_segq) != NULL) {
+    	th = NULL;
+		mtag = NULL;
+    	mp->mp_input_segq = m->m_nextpkt;
+
+    	/* XXXNJW: A mbuf with a m_tag has length but no header.
+    	 * A mbuf with a tcp header will not have any length (beyond
+    	 * that of the header) */
+    	mtag = m_tag_locate(m, PACKET_COOKIE_MPTCP, PACKET_TAG_DSN, NULL);
+        if (mtag) {
+            tlen = m->m_pkthdr.len;
+            dtag = (struct dsn_tag *) mtag;
+
+            if (dtag->dss_flags & MPD_DSN32) {
+            	dtag->dsn = mp_dsn32to64(dtag->dsn, mp->ds_rcv_nxt);
+			}
+        } else {
+            ip = mtod(m, struct ip *);
+		    th = (struct tcphdr *)((caddr_t)ip + sizeof(struct ip));
+
+            /* Only expect that DACK, other mp signal segments
+             * will arrive with tcphdrs. In this case tlen will
+             * be the length of the headers, and adjusting tlen
+             * back by offset will result on a tlen of 0.
+             *
+             * Hence it's probably not nessesary to bother with
+             * setting tlen here, or even testing if the offset
+             * exceeds the length of the header (would have been
+             * checked in tcp_input anyway). */
+		    tlen = ntohs(ip->ip_len) - sizeof(struct ip);
+            off = th->th_off << 2;
+	        if (off < sizeof (struct tcphdr) || off > tlen) {
+	            m_free(m);
+                continue;
+            }
+            tlen -= off;
+        }
+
+    	/* mp_do_segment consumes the mbuf, returns MPP_LOCKED if
+    	 * return is 1, otherwise MPP is unlocked. mbuf consumed
+    	 * prior to return */
+    	mp_do_segment(m, th, so, mp, tlen);
+    }
+}
+
+static int
+mp_do_segment(struct mbuf *m, struct tcphdr *th, struct socket *so,
+    struct mpcb *mp, int tlen)
+{
+	int need_output = 0;
+	int flags = 0;
+    struct tcpopt to;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* parse options if we have a tcp hdr. We only ever process
+     * options in these mbufs - there should never be data */
+	if (th) {
+		mp_dooptions(&to, (u_char *)(th + 1),
+			(th->th_off << 2) - sizeof(struct tcphdr));
+
+		/*
+		 * 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 addresses that a client
+		 * can connect to (though a client may advertise a new address and then
+		 * connect _from_ that address).
+		 */
+		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 */
+            printf("%s: got new addr\n", __func__);
+
+			bcopy(&to.to_mopts.new_addr,
+				&mp->mp_added_addresses[to.to_mopts.addr_id],
+				sizeof(struct sockaddr_storage));
+
+			mp->mp_added_addresses[to.to_mopts.addr_id].ss_family =
+				AF_INET;
+			mp->mp_added_addresses[to.to_mopts.addr_id].ss_len =
+				sizeof(struct sockaddr_in);
+
+			char buf[128];
+			inet_ntop(AF_INET, &(((struct sockaddr_in *)
+				&mp->mp_added_addresses[to.to_mopts.addr_id])->sin_addr.s_addr),
+				buf, sizeof(buf));
+			mp_debug(MPSESSION, 4, 0, "added address %s\n", buf);
+
+			inet_ntop(AF_INET, &(((struct sockaddr_in *)
+				&to.to_mopts.new_addr)->sin_addr.s_addr),
+				buf, sizeof(buf));
+			mp_debug(MPSESSION, 4, 0, "got address %s\n", buf);
+			printf("%s: got address %s\n", __func__, buf);
+
+			/* XXXNJW: TEMP - Need to implement improved path management */
+			/* update address count and mask */
+			mp->mp_lrnedjoin_mask |=
+				(1 << (mp->mp_added_address_count));
+			mp->mp_added_address_count += 1;
+		}
+
+    	if (to.to_mopts.mpo_flags & MPOF_DATA_ACK) {
+    		to.to_mopts.data_ack_num =
+    			mp_ack32to64(to.to_mopts.data_ack_num, mp->ds_snd_una);
+
+    		if (to.to_mopts.data_ack_num > mp->ds_snd_una) {
+    		    if (mp_data_ack(mp, to.to_mopts.data_ack_num))
+    			    need_output = 1;
+
+				/* Process DFIN-ACK */
+				if(mp->mp_flags & MPF_SENTDFIN) {
+
+	//				printf("%s: process dfin-ack dack %u una %u snd_max %u equal %d\n",
+	//					__func__, (uint32_t)to.to_mopts.data_ack_num,
+	//					(uint32_t) mp->ds_snd_una, (uint32_t) mp->ds_snd_max,
+	//					(mp->ds_snd_una == mp->ds_snd_max) ? 1 : 0);
+	//				printf("%s: 64-bit ack %ju una %ju snd_max %ju snd_nxt %ju\n",
+	//						__func__, to.to_mopts.data_ack_num, mp->ds_snd_una,
+	//						mp->ds_snd_max, mp->ds_snd_nxt);
+
+					if (mp->ds_snd_una == mp->ds_snd_max) {
+						switch (mp->mp_state) {
+						case MPS_M_FIN_WAIT_1:
+							if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
+								soisdisconnected(so);
+							mp->mp_state = MPS_M_FIN_WAIT_2;
+							mp_close_all_subflows(mp);
+							goto drop;
+						case MPS_M_CLOSING:
+							mp_twstart(mp);
+							mp_close_all_subflows(mp);
+							goto drop;
+						case MPS_M_LAST_ACK:
+							mp->mp_mppcb->mpp_flags |= MPP_DROPPED;
+							mp->mp_state = MPS_M_CLOSED;
+							mp_close_all_subflows(mp);
+							goto drop;
+						}
+						printf("%s: dfin is acked, state now %d needoutput %d\n",
+							__func__, mp->mp_state, need_output);
+					}
+				}
+    		} // else this is an out-of-date DACK.
+    	}
+
+    	/* No longer needed */
+		m_free(m);
+
+	} else if (th == NULL) {
+        /* These are pure data segments with a DSN tag. Any MPTCP
+         * header options will be processed in the block above.
+         * (this means that we enqueue two mbufs for a segment
+         * with data + MPTCP signalling). */
+
+		flags = mp_reass(mp, m);
+
+		/* XXXNJW: always D-ack for now. */
+		mp->mp_flags |= MPF_ACKNOW;
+
+		/* Process DFIN on segment */
+		if (flags & MP_DFIN) {
+			/* A DFIN on a segment with no length, need to bump rcv_nxt
+			 * XXXNJW: would still need to bump the rcv_nxt, even if
+			 * there was data? */
+			if (MPS_HAVERCVDFIN(mp->mp_state) == 0) {
+				printf("%s: first dfin, state %d\n", __func__, mp->mp_state);
+				// XXXNJW: need to take care of half-syncd connections
+				// that get a DFIN? (we won't even attempt to process
+				// these in the current code, as MPS would be < ESTAB
+
+				socantrcvmore(so);
+				mp->ds_rcv_nxt++;
+				need_output = 1;
+			}
+			switch (mp->mp_state) {
+			case MPS_M_ESTABLISHED:
+				mp->mp_state = MPS_M_CLOSE_WAIT;
+				break;
+
+			case MPS_M_FIN_WAIT_1:
+				mp->mp_state = MPS_M_CLOSING;
+				break;
+
+			case MPS_M_FIN_WAIT_2:
+				mp_twstart(mp);
+				break;
+			}
+			printf("%s: DFIN, set state %d need_output %d\n",
+				__func__, mp->mp_state, need_output);
+		}
+	} else /* No signaling, no data to deliver */
+		goto drop;
+
+	/* XXXNJW: modified to call mp_output per-segment (as required)
+	 * This is an expensive thing to do, so delaying DACKs here and
+	 * coalescing contiguous data-level segments prior to calling
+	 * mp_do_segment needs to be implemented. */
+	if (need_output)
+		mp_output(mp);
+
+	return 0;
+drop:
+	m_freem(m);
+    return 0;
+}
+
+/* Standard TCP flows */
+int mp_standard_output(struct mpcb *mp)
+{
+    int error = 0;
+    struct socket *so = mp->mp_mppcb->mpp_socket;
+    struct sf_handle *sf = TAILQ_FIRST(&mp->sf_list);
+
+    KASSERT(sf != NULL, ("%s sf == NULL\n", __func__));
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+    error = mp_do_output(so, mp, sf, 0);
+
+    return error;
+}
+
+/* Multipath TCP flows */
+int mp_output(struct mpcb *mp)
+{
+	struct socket *so = mp->mp_mppcb->mpp_socket;
+	struct sf_handle *sf = NULL;
+    int flags = 0, error = 0;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	/* how might we get into this situation? */
+	if (mp->mp_state == MPS_M_CLOSED) {
+		printf("%s mp_output mp %p, state MPS_CLOSED so %p\n", __func__,
+		    mp, so);
+		//mp_disconnect_all_subflows(mp);
+		goto out;
+	}
+
+	if (mp->mp_state >= MPS_M_ESTABLISHED)
+		flags = mp_outflags[mp->mp_state];
+
+	/* If we've sent a DFIN already, and ds_snd_nxt isn't ds_snd_una, then
+	 * this isn't a rexmit and we should unset the flag.
+	 * XXXNJW:
+	 */
+	if ((flags & MP_DFIN) &&
+	   ((mp->mp_flags & MPF_SENTDFIN) && !(mp->ds_snd_nxt == mp->ds_snd_una)))
+        flags &= ~MP_DFIN;
+
+	/* XXXNJW: Prevent adding another byte for the DFIN. Seems to be
+	 * a rexmit case for the DFIN. when is this condition met? a rexmit
+	 * would not equal ds_snd_max? */
+	if ((flags & MP_DFIN) && (mp->mp_flags & MPF_SENTDFIN) &&
+	    mp->ds_snd_nxt == mp->ds_snd_max) {
+		mp->ds_snd_nxt--;
+	}
+
+	if (mp->mp_flags & MPF_ACKNOW) {
+		mp->mp_flags &= ~MPF_ACKNOW;
+		flags |= MP_DACK;
+	}
+
+	if (mp->mp_state > MPS_M_ESTABLISHED)
+		printf("%s: mp %p in state %d flags %d\n",
+		    __func__, mp, mp->mp_state, flags);
+
+	/* Temporary address management (advertising, scheduling joins..) */
+	if (mp->mp_state == MPS_M_ESTABLISHED) {
+		/* Are there local addresses that should be advertised to the remote
+		 * host? */
+		if (mp->mp_advaddr_mask)
+			flags |= MP_ADV_ADDR;
+
+	    /* XXXNJW: basic joining logic. Schedule join task if:
+		 * - Are an active opener (client) and have no addresses to advertise
+		 * - Have not yet issued MP_JOINs to addresses we have advertised OR
+		 *   have learned about via ADD_ADDR
+		 */
+		if (!mp->mp_passive && (mp->mp_advaddr_mask == 0) &&
+		    ((mp->mp_advjoin_mask) || (mp->mp_lrnedjoin_mask))) {
+			mp_schedule_tasks(mp, MP_SCHEDJOIN);
+		}
+	}
+
+	/* XXXNJW: Reset the flags if there is a reason */
+
+	/* XXXNJW: There may be some instances where we do not want to send
+	 * anything further. In that case we do not do subflow selection and
+	 * should return (unlocked?) */
+
+	/* NB: the length is being overridden to be all unmapped chars. should
+	 * however ensure that we don't append more than what the subflow can
+	 * fit in the send buffer */
+
+	/* XXXNJW: Temp while testing */
+	/* Select a subflow. A scheduler hook would go here. The scheduler should
+	 * return with a locked inp */
+	sf = mp_get_subflow(mp);
+
+	if (mp->mp_state > MPS_M_ESTABLISHED)
+		printf("%s: selected tp %p\n",__func__, sotoinpcb(sf->sf_so)->inp_ppcb);
+
+	/* No subflow was available to send data (e.g. all disconnected) */
+	if (sf == NULL && mp->mp_state == MPS_M_ESTABLISHED) {
+		/* start rexmit timer to try again later */
+		if (!mp_timer_active(mp, MPT_REXMT))
+			mp_timer_activate(mp, MPT_REXMT, mp->mp_rxtcur);
+		goto out;
+	} else if (sf == NULL && mp->mp_state >= MPS_M_ESTABLISHED) {
+	    /* If we are disconnecting and no longer have any subflows to
+	     * send data on, might as well close now. */
+
+		/* Since we can't send anything, start a protocol disconnect on
+		 * all the subflows, ignoring the data level shutdown. The mpcb
+		 * will be freed once the subflows are disconnected. */
+		mp_close_all_subflows(mp);
+		goto out;
+	}
+
+	KASSERT(sf != NULL, ("%s: Subflow handle NULL\n", __func__));
+	error = mp_do_output(so, mp, sf, flags);
+
+out:
+    return (error);
+
+}
+
+/* XXXNJW: should deal with errors from the sending subflow (though we
+ * don't want to propagate these back to the socket) */
+static int
+mp_do_output(struct socket *so, struct mpcb *mp, struct sf_handle *sf,
+    int flags)
+{
+	struct inpcb *inp;
+	struct tcpcb *tp;
+	struct ds_map *map;
+    struct mbuf *m_mapped, *mb;
+    int error = 0;
+    int moff, off, map_length = 0;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	/* Interlock the subflow inp to ensure the inp doesn't
+	 * disappear when we drop the MP_LOCK. */
+	inp = sotoinpcb(sf->sf_so);
+	KASSERT(inp != NULL, ("%s: inp == NULL mp %p so %p sfh %p", __func__, mp,
+	    so, sf));
+
+	/* A temporary thing while trying to figure out a race condition */
+	if (!mp->mp_connected) {
+		INP_WLOCK(inp);
+		if ((inp->inp_flags & INP_DROPPED) || (inp->inp_flags & INP_TIMEWAIT)) {
+			printf("%s: inp dropped mid-send\n", __func__);
+			INP_WUNLOCK(inp);
+			goto unlock;
+		}
+		tp = intotcpcb(inp);
+		uint32_t acknum = tp->last_ack_processed = tp->snd_una;
+		INP_WUNLOCK(inp);
+
+		if (acknum > mp->ds_snd_una)
+		    mp_data_ack(mp, acknum);
+	}
+
+	SOCKBUF_LOCK(&so->so_snd);
+
+	/* What we send next depends on whether we are retransmitting
+	 * or sending new data. check the state of the mp and set the
+	 * "send next" according to this (either snd_max for new, or
+	 * snd_una for a rexmit).
+	 *
+	 * How much should we transmit initially? as there aren't any
+	 * mappings at the data level. just a few segments?
+	 */
+
+	/* XXXNJW temp fix for disconnected MP flows that still have a
+	 * byte in the send buffer (but no sb_mb for some reason...)
+	 * need to figure out why one byte would be in there when the
+	 * buffer has no sb_mb.
+	 *
+	 * This is a little different from the -len issue that occurs
+	 * in standard TCP once a FIN has been sent (but not acked,
+	 * i.e. snd_nxt is '1' greater than snd_una), as in this case
+	 * there actually appears to be a single byte in so_snd.
+	 *
+	 * NB: the "MPS_ESTABLISHED" check is the fix.*/
+	if (!mp->mp_connected || mp->mp_state >= MPS_M_ESTABLISHED) {
+        off = mp->ds_snd_nxt - mp->ds_snd_una;
+        map_length = sbavail(&so->so_snd) - off;
+
+        if (map_length < 0) {
+			printf("%s: sb_avail %d map_len %d off %d\n", __func__,
+				sbavail(&so->so_snd), map_length, off);
+			printf("%s: ds_snd_una %u ds_snd_nxt %u\n",
+					__func__, (uint32_t) mp->ds_snd_una,
+					(uint32_t) mp->ds_snd_nxt);
+			printf("%s: ds_snd_max %u\n", __func__, (uint32_t) mp->ds_snd_max);
+		}
+
+        /* XXXNJW: If we have mapped new data from the sb, then we don't
+         * want to include a DFIN. This restricts us to only putting a
+         * DFIN on an empty DSS. But it also means that DFINs won't be
+         * put onto rexmits, for example. */
+        if ((flags & MP_DFIN) && map_length) {
+			printf("%s: avail %d map_length %d ds_snd_una %u ds_snd_nxt %u\n",
+				__func__, sbavail(&so->so_snd),	map_length,
+				(uint32_t) mp->ds_snd_una, (uint32_t) mp->ds_snd_nxt);
+            flags &= ~MP_DFIN;
+        }
+	}
+
+	/* Ensure that SENTDFIN set? (just while debugging?) */
+	if (map_length < 0) {
+		/*
+		 * If FIN has been sent but not acked,
+		 * but we haven't been called to retransmit,
+		 * len will be < 0. In transmit case
+	     * snd_nxt == snd_una so off is '0'
+		 */
+		map_length = 0;
+	}
+
+	KASSERT(map_length >= 0, ("%s: offset < 0\n", __func__));
+
+	/* In retransmit case, let's send half of the outstanding data. */
+	if (DSEQ_LT(mp->ds_snd_nxt, mp->ds_snd_max) &&
+	    !((mp->mp_flags & MPF_SENTDFIN))) {
+		printf("%s: retransmitting from %u, shift %d maplen %d\n", __func__,
+		    (uint32_t)mp->ds_snd_una, mp->mp_rxtshift, map_length);
+	}
+
+	/* XXNJW: Possible to get calls that don't have any new data
+	 * to send, and no signaling is required. */
+	if (!map_length && !flags) {
+		SOCKBUF_UNLOCK(&so->so_snd);
+		goto unlock;
+	}
+
+    if (map_length) {
+    	if (flags & MP_DFIN)
+    	    flags &= ~MP_DFIN;
+
+		map = mp_get_map(mp, map_length);
+		if (!map) {
+			error = ENOBUFS;
+			SOCKBUF_UNLOCK(&so->so_snd);
+			goto unlock;
+		}
+
+		/*
+		 * Start the m_copym function from the closest mbuf
+		 * to the offset in the socket buffer chain.
+		 */
+		mb = sbsndptr(&so->so_snd, off, map_length, &moff);
+
+		/* copy the mapped data from the data-level send buffer to a new mbuf
+		 * chain */
+		m_mapped = m_copym(mb, moff, map_length, M_NOWAIT);
+		if (m_mapped == NULL) {
+			/* XXXNJW Need to remember the map that we've allocated, so that
+			 * when we try again that map is copied and transmitted first
+			 * OR should just free the map now, and a new map can be alloc'd
+			 * on the next call through (as ds_snd_nxt will not have been
+			 * incremented). */
+			error = ENOBUFS;
+			SOCKBUF_UNLOCK(&so->so_snd);
+			goto unlock;
+	    }
+
+		SOCKBUF_UNLOCK(&so->so_snd);
+
+		mp->ds_snd_nxt += map_length;
+
+		/* XXXNJW: advance snd_nxt */
+		if (flags & MP_DFIN) {
+		    mp->mp_flags |= MPF_SENTDFIN;
+		    mp->ds_snd_nxt++;
+		}
+
+		/* Increment ds_snd_max if we've mapped new data
+		 * and are about to send */
+		if (DSEQ_GT(mp->ds_snd_nxt, mp->ds_snd_max))
+			mp->ds_snd_max = mp->ds_snd_nxt;
+
+		if (mp->mp_connected && !mp_timer_active(mp, MPT_REXMT))
+			mp_timer_activate(mp, MPT_REXMT, mp->mp_rxtcur);
+
+		/* XXXNJW - the state on an inp might have changed since
+		 * it was nominated for output. need a suitable way to handle
+		 * this case as at this point we've already mapped and
+		 * accounted for the data we are going to send. (though the
+		 * rexmit timer will eventually fire if we return at this
+		 * point */
+		INP_WLOCK(inp);
+		if ((inp->inp_flags & INP_DROPPED) || (inp->inp_flags & INP_TIMEWAIT)) {
+			printf("%s: inp dropped mid-send\n", __func__);
+			INP_WUNLOCK(inp);
+			goto unlock;
+		}
+
+		tp = intotcpcb(inp);
+		tp->t_mp_conn.ds_ack_num = mp->ds_rcv_nxt;
+		tp->t_mp_conn.ds_snd_nxt = mp->ds_snd_nxt; // XXX: ??
+
+		struct ds_map *previous_map;
+		previous_map = TAILQ_LAST(&tp->t_send_maps.dsmap_list, dsmapq_head);
+		if (previous_map)
+			map->sf_seq_start = previous_map->sf_seq_start
+			    + previous_map->ds_map_len;
+		else
+			map->sf_seq_start = tp->snd_nxt;
+
+		/* XXXNJW: TODO If there is ds-level overlap, should trim the
+		 * retransmit map to save sending the same data twice. */
+
+		/* Insert the allocated map at the tail. Maps will always have
+		 * increasing sf_seq_start, and will not overlap in subflow sequence
+		 * space */
+		map->sf_tp = tp;
+		TAILQ_INSERT_TAIL(&tp->t_send_maps.dsmap_list, map, sf_ds_map_next);
+
+		/* Append to the mapped data to the subflow */
+		sbappendstream(&inp->inp_socket->so_snd, m_mapped, 0);
+
+		/* The map mbuf pointers are not useful now */
+		/* XXXNJW: perhaps keeping a reference to the first mbuf might be useful? */
+		map->mbuf_start = m_mapped;
+		map->mbuf_offset = 0;
+
+		/* start the ds-level timer here */
+	} else {
+		/* XXXNJW: advance snd_nxt */
+		if (flags & MP_DFIN) {
+			mp->mp_flags |= MPF_SENTDFIN;
+			mp->ds_snd_nxt++;
+		}
+
+		if (DSEQ_GT(mp->ds_snd_nxt, mp->ds_snd_max))
+			mp->ds_snd_max = mp->ds_snd_nxt;
+
+		SOCKBUF_UNLOCK(&so->so_snd);
+
+		INP_WLOCK(inp);
+		if ((inp->inp_flags & INP_DROPPED) || (inp->inp_flags & INP_TIMEWAIT)) {
+			printf("%s: inp dropped mid-send\n", __func__);
+			/* if we are trying to send DFIN for the first time */
+			if ((flags & MP_DFIN) && (mp->mp_flags & MPF_SENTDFIN))
+				mp->mp_flags &= ~MPF_SENTDFIN;
+			INP_WUNLOCK(inp);
+			goto unlock;
+		}
+
+		tp = intotcpcb(inp);
+		tp->t_mp_conn.ds_ack_num = mp->ds_rcv_nxt;
+		tp->t_mp_conn.ds_snd_nxt = mp->ds_snd_nxt; // XXX: ??
+
+//		MPP_UNLOCK(mp->mp_mppcb);
+
+		/* If we are only sending MPTCP signaling, need to force
+		 * the subflow to send a packet. */
+		if (flags)
+		    tp->t_flags |= TF_ACKNOW;
+
+	}
+
+	/* Flags passed to the subflow.
+	 * XXXNJW: There are additional SFFs to could set in here */
+    if (flags & MP_ADV_ADDR)
+    	tp->t_sf_flags |= SFF_SEND_ADD_ADDR;
+    if (flags & MP_DACK)
+		tp->t_sf_flags |= SFF_NEED_DACK;
+	if (flags & MP_DFIN)
+		tp->t_sf_flags |= SFF_NEED_DFIN;
+
+	/* call output if there is data to send, or we need to send control info */
+	if(map_length || flags)
+	    error = tcp_output(tp);
+
+	INP_WUNLOCK(inp);
+//	return error;
+
+unlock:
+//	MPP_UNLOCK(mp->mp_mppcb);
+	return error;
+
+}
+
+/* XXXNJW: temporary round robin scheduler. just using the link field in the
+ * subflow handle so select subflows in order of insertion. When we get to
+ * the end just select the first subflow.
+ *
+ * Might need to call into this distinguishing between whether we need to
+ * send data, or just MP-level signaling. In the case of signaling it is
+ * possible to use subflows that are in any state other than < EST or TW
+ * (in the TW case, inp will be set as INP_DROPPED)
+ * */
+static struct sf_handle *
+mp_get_subflow(struct mpcb *mp)
+{
+	struct sf_handle *sf_index = NULL;
+	struct sf_handle *sf_next = NULL;
+    struct inpcb *inp;
+    struct tcpcb *tp;
+
+	/* The last subflow used for output */
+	sf_index = mp->mp_temp_sched.last_sf_selected;
+
+	/* want to start from the "next" subflow after our
+	 * previously used subflow. */
+	if (sf_index)
+		sf_next = TAILQ_NEXT(sf_index, next_sf_handle);
+
+	/* will start from the start of list */
+	if (sf_next == NULL)
+		sf_index = sf_next = TAILQ_FIRST(&mp->sf_list);
+
+again:
+	TAILQ_FOREACH_FROM(sf_next, &mp->sf_list, next_sf_handle) {
+		if (sf_next->sf_flags & (SFHS_MPENDED | SFHS_MPESTABLISHED))
+			continue;
+
+		/* XXXNJW: some cases can drop through without an inp.
+		 * need to investigate why. */
+
+		/* Rather than subflow-level checks, should in the future rely
+		 * only on sfh flags. If there is some problem with the PCB
+		 * the calling function can try again. */
+		inp = sotoinpcb(sf_next->sf_so);
+		INP_WLOCK(inp);
+		if (inp->inp_flags & (INP_DROPPED | INP_TIMEWAIT)) {
+			sf_next->sf_flags |= SFHS_MPENDED;
+
+			/* XXXNJW: this just sneaked in for now to catch
+			 * flows that have (for example) been reset. Must
+			 * find a better solution for this. */
+			if (tp->t_sf_state & SFS_MP_DISCONNECTED)
+				mp_schedule_tasks(mp, MP_SCHEDCLOSE);
+
+			INP_WUNLOCK(inp);
+			continue;
+		}
+		tp = intotcpcb(inp);
+		if ((tp->t_state < TCPS_ESTABLISHED) || tp->t_rxtshift) {
+			INP_WUNLOCK(inp);
+			continue;
+		}
+
+		INP_WUNLOCK(inp);
+		break;
+	}
+
+	if ((sf_next == NULL) && (sf_index != TAILQ_FIRST(&mp->sf_list))) {
+		sf_index = TAILQ_FIRST(&mp->sf_list);
+		goto again;
+	}
+
+	mp->mp_temp_sched.last_sf_selected = sf_next;
+	return sf_next;
+}
+
+/*
+ * Free everything. locks, lists etc etc
+ */
+void
+mp_discardcb(struct mpcb *mp)
+{
+	/* Release any ds_maps that remain */
+    struct mppcb *mpp = mp->mp_mppcb;
+
+    MPP_LOCK_ASSERT(mpp);
+
+    SDT_PROBE1(mptcp, session, mp_discardcb, entry, mp);
+    printf("%s: %p\n", __func__, mp);
+
+
+	/* Release any subflow handles. */
+	mp_sf_flush(mp);
+	/* Release any recorded socket options */
+	mp_mpsopt_flush(mp);
+	/* Release any segments remaining in the reass queue */
+    mp_reass_flush(mp);
+
+	/* Release mpti entry */
+//	/mp_remtoklist(mp->local_token);
+
+//	taskqueue_drain(mp->mp_tq, &mp->rexmit_task);
+//	taskqueue_drain(mp->mp_tq, &mp->data_task);
+//	taskqueue_drain(mp->mp_tq, &mp->sf_mgmt_task);
+//	taskqueue_free(mp->mp_tq);
+
+    /* XXXNJW: again, a hack to cancel enqueued tasks
+     * now that the mpcb is discarded. */
+
+    u_int pend = 0;
+	taskqueue_cancel(taskqueue_swi, &mp->subflow_event_task, &pend);
+
+	taskqueue_cancel(taskqueue_swi, &mp->subflow_detached_task, &pend);
+	if (pend) {
+	    printf("%s: subflow_detached_task was pending", __func__);
+	    pend = 0;
+	}
+
+	taskqueue_cancel(taskqueue_swi, &mp->subflow_close_task, &pend);
+	if (pend) {
+		printf("%s: subflow_close_task was pending", __func__);
+		pend = 0;
+	}
+
+	taskqueue_cancel(taskqueue_swi, &mp->mp_output_task, &pend);
+	taskqueue_cancel(taskqueue_swi, &mp->mp_input_task, &pend);
+	taskqueue_cancel(taskqueue_swi, &mp->join_task, &pend);
+
+	callout_stop(&mp->mp_timers->mpt_rexmt);
+	callout_stop(&mp->mp_timers->mpt_timeout);
+	free(mp->mp_timers, M_MPTIMERS);
+
+	mpp->mpp_mpcb = NULL;
+	uma_zfree(V_mpcb_zone, mp);
+}
+
+/*
+ * Return the common rcv window. The window is updated when soreceive is called
+ * and is attached to the subsequent outgoing DACK that signifies that data
+ * has been read by the application.
+ */
+uint32_t
+mp_get_recwin(struct mpcb *mp) {
+	mp_update_recwin(mp);
+	return mp->ds_rcv_wnd;
+}
+
+static void
+mp_update_recwin(struct mpcb *mp) {
+	mp->ds_rcv_wnd = sbspace(&mp->mp_mppcb->mpp_socket->so_rcv);
+}
+
+void
+mp_update_sndwin(struct mpcb *mp, uint16_t win) {
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+	mp->ds_snd_wnd = win;
+}
+
+/* Try to locate an existing entry for a socket option that
+ * is being set. If we can't find an entry, then this is the
+ * first time we have seen this option. */
+struct mp_sopt*
+mp_locate_mp_sopt(struct mpcb *mp, struct sockopt *sopt)
+{
+	 struct mp_sopt *m_sopt = NULL;
+
+	 MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	 TAILQ_FOREACH(m_sopt, &mp->mp_sopt_list, next_mp_sopt) {
+		   if (m_sopt->sopt_level == sopt->sopt_level &&
+			   m_sopt->sopt_name == sopt->sopt_name)
+				  break;
+	 }
+
+     return m_sopt;
+}
+
+struct mp_sopt*
+mp_alloc_mp_sopt(void)
+{
+    struct mp_sopt *mpsopt;
+//    mpsopt = uma_zalloc(mpsopt_zone, M_NOWAIT | M_ZERO);
+    mpsopt = malloc(sizeof(struct mptcp_timer), M_MPSOPT, M_NOWAIT);
+    return mpsopt;
+}
+
+void
+mp_schedule_tasks(struct mpcb *mp, int task_flags)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	if ((task_flags & MP_SCHEDINPUT)) {
+		if (mp->mp_input_pending == 0) {
+			mp->mp_input_pending = 1;
+			mpp_pcbref(mp->mp_mppcb);
+			taskqueue_enqueue(taskqueue_swi, &mp->mp_input_task);
+		}
+	}
+
+	if ((task_flags & MP_SCHEDEVENT)) {
+		if(mp->mp_event_pending == 0) {
+			mp->mp_event_pending = 1;
+			mpp_pcbref(mp->mp_mppcb);
+			taskqueue_enqueue(taskqueue_swi, &mp->subflow_event_task);
+		}
+	}
+
+	if ((task_flags & MP_SCHEDJOIN)) {
+		printf("%s: enqueue subflow join\n", __func__);
+		if (mp->mp_join_pending == 0) {
+			mp->mp_join_pending = 1;
+			mpp_pcbref(mp->mp_mppcb);
+			taskqueue_enqueue(taskqueue_swi, &mp->join_task);
+		}
+	}
+
+//	if ((task_flags & MP_SCHEDCLOSE)) {
+//		if (mp->mp_sf_close_pending == 0) {
+//			mp->mp_sf_close_pending = 1;
+//			mpp_pcbref(mp->mp_mppcb);
+//			taskqueue_enqueue(taskqueue_swi, &mp->subflow_close_task);
+//		}
+//	}
+
+}
+
+/* XXXNJW: Previously were done asynchronously but were running
+ * into race conditions with taking mpp_pcbref and the two pending
+ * flags. So now just run them in this thread while we have the MPP
+ * anyway. */
+int
+mp_do_task_now(struct mpcb *mp, int task_flags)
+{
+	int unlocked = 0;
+
+	if ((task_flags & MP_SCHEDCLOSE))
+		mp_close_subflow(mp);
+
+	if ((task_flags & MP_SCHEDDETACH))
+		 unlocked = mp_subflow_detached(mp, 1);
+
+	return unlocked;
+}
+
+/* An event has occurred on a subflow that the mp-layer should handle.
+ * If we haven't enqueued the task, do so now otherwise just set the
+ * event flags. */
+void
+mp_enqueue_subflow_event(struct tcpcb *tp, u_int16_t event_flag)
+{
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+
+//	tp->t_event_flags |= event_flag;
+//	if(tp->t_event_pending == 0) {
+//		tp->t_event_pending = 1;
+//		taskqueue_enqueue(taskqueue_swi, &tp->t_mpcb->subflow_event_task);
+//		if (atomic_cmpset_int(&tp->t_mpcb->mp_sf_event_pending, 0, 1))
+//			mpp_pcbref(tp->t_mpcb->mp_mppcb);
+//	}
+}
+
+//void
+//mp_enqueue_event(struct mpcb *mp, u_int16_t event_flag)
+//{
+//	MPP_LOCK_ASSERT(mp->mp_mppcb);
+//
+//	mp->mp_event_flags |= event_flag;
+//	taskqueue_enqueue(taskqueue_swi, &mp->mp_event_task);
+//	if (atomic_cmpset_int(&mp->mp_sf_event_pending, 0, 1))
+//		mpp_pcbref(mp->mp_mppcb);
+//
+//}
+
+static void
+mp_sf_connected(struct mpcb *mp, struct tcpcb *tp)
+{
+	struct socket *so;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	so = mp->mp_mppcb->mpp_socket;
+	KASSERT(so != NULL, ("%s: so NULL\n", __func__));
+
+	/* Set the primary socket as connected */
+	if (so->so_state & SS_ISCONNECTING) {
+		printf("%s: set primary socket %p connected\n", __func__, so);
+
+		/* Init the sequence numbers based on the subflow that
+		 * just connected, if MP is not established at this point.
+		 * The connection will commence using subflow sequence
+		 * space. */
+		if (!mp->mp_connected) {
+            mp_sendseqinit(mp,tp);
+		    mp_rcvseqinit(mp, tp);
+		}
+
+		soisconnected(so);
+	}
+
+}
+
+
+void
+mp_input_task_handler(void *context, int pending)
+{
+	struct mpcb *mp;
+	struct mppcb *mpp;
+	struct socket *so;
+
+	mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+
+	MPP_LOCK(mpp);
+
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: mpp freed, return\n", __func__);
+		return;
+	}
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	mp->mp_input_pending = 0;
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		goto out;
+	}
+
+	so = mpp->mpp_socket;
+	KASSERT(so != NULL, ("%s: so from context was NULL\n", __func__));
+
+//	/* mp_input unlocks MPP */
+	if (mp->mp_input_segq)
+		mp_input(mp, so);
+	else if (!mp->mp_connected)
+		mp_standard_input_ack(mp, so);
+
+out:
+	MPP_UNLOCK(mpp);
+}
+
+
+/* Drop from the primary send buffer here under MPP_LOCK to ensure stability
+ * when assigning new maps/data to subflows. (accounting is adjusted on
+ * the subflow input path) */
+void
+mp_output_task_handler(void *context, int pending)
+{
+
+	struct mpcb *mp;
+	struct mppcb *mpp;
+    struct socket *so;
+
+    mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_refcount < 2)
+	    printf("%s: mpp ref count %d\n", __func__, mpp->mpp_refcount);
+
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: so %p mpp %p mp %p\n", __func__, mpp->mpp_socket, mpp,
+			mpp->mpp_mpcb);
+		return;
+	}
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	mp->mp_output_pending = 0;
+
+	so = mpp->mpp_socket;
+	KASSERT(so != NULL, ("%s: so from context was NULL\n", __func__));
+
+	/* XXXNJW: probably should peek at any errors */
+	if (mp->mp_connected)
+	    mp_output(mp);
+	else
+		mp_standard_output(mp);
+
+	MPP_UNLOCK(mpp);
+}
+
+void
+mp_subflow_event_task_handler(void *context, int pending)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	struct sf_handle *sfhandle;
+	struct inpcb *inp;
+	struct tcpcb *tp;
+
+	mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_refcount < 2)
+	    printf("%s: mpp ref count %d\n", __func__, mpp->mpp_refcount);
+
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: so %p mpp %p mp %p\n", __func__, mpp->mpp_socket, mpp,
+			mpp->mpp_mpcb);
+		return;
+	}
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	mp->mp_sf_event_pending = 0;
+
+	TAILQ_FOREACH(sfhandle, &mp->sf_list, next_sf_handle) {
+		if (sfhandle->sf_flags & SFHS_MPENDED)
+		    continue;
+
+		inp = sotoinpcb(sfhandle->sf_so);
+		KASSERT(inp != NULL, ("%s: inp NULL\n", __func__));
+		INP_WLOCK(inp);
+
+		if (inp->inp_flags & INP_TIMEWAIT) {
+			INP_WUNLOCK(inp);
+			continue;
+		}
+
+		tp = intotcpcb(inp);
+		if (tp->t_event_pending == 0) {
+			INP_WUNLOCK(inp);
+			continue;
+		}
+
+		tp->t_event_pending = 0;
+		mp_process_subflow_event(mp, tp);
+		INP_WUNLOCK(inp);
+	}
+
+	MPP_UNLOCK(mpp);
+}
+
+void
+mp_join_task_handler(void *context, int pending)
+{
+	struct mpcb *mp;
+	struct mppcb *mpp;
+    struct socket *so;
+
+	mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_refcount < 2)
+	    printf("%s: mpp ref count %d\n", __func__, mpp->mpp_refcount);
+
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: so %p mpp %p mp %p\n", __func__, mpp->mpp_socket, mpp,
+			mpp->mpp_mpcb);
+		return;
+	}
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	mp->mp_join_pending = 0;
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED))
+		goto unlock_return;
+
+	if (mp->subflow_cnt == MAX_SUBFLOWS || mp->mp_state < MPS_M_ESTABLISHED)
+		goto unlock_return;
+
+	so = mp->mp_mppcb->mpp_socket;
+	if (!(so->so_state & SS_ISCONNECTED))
+		goto unlock_return;
+
+	/* Addresses remain to join from */
+	if (mp->mp_advjoin_mask)
+		mp_join_from_advertised(mp, so);
+
+	/* Learned addresses remain to join to */
+	if (mp->mp_lrnedjoin_mask)
+		mp_join_learned(mp, so);
+
+unlock_return:
+	MPP_UNLOCK(mp->mp_mppcb);
+
+}
+
+static int
+mp_join_from_advertised(struct mpcb *mp, struct socket *so)
+{
+	struct inpcb *inp;
+	void *laddr = NULL, *faddr = NULL;
+	u_int16_t lport, fport;
+	int error = 0;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	/* Have advertised addresses that we have not yet tried to the
+	 * default foreign interface from from. */
+	char buf[128];
+	if (mp->mp_advjoin_mask) {
+		/* try connect to default foreign address */
+		lport = mp->mp_mppcb->mpp_lport;
+		fport = mp->mp_mppcb->mpp_fport;
+		faddr = &((struct sockaddr_in *)
+			&mp->mp_foreign_address)->sin_addr.s_addr;
+
+		inet_ntop(AF_INET, faddr, buf, sizeof(buf));
+		printf("%s: faddr now %s:%d\n", __func__, buf, fport);
+
+		int i;
+		for (i = 1; i < mp->mp_conn_address_count; i++) {
+			printf("%s: conn addr count %d\n", __func__, i);
+
+			/* continue if this index has sent join already */
+			if (!(mp->mp_advjoin_mask & (1 << i)))
+				continue;
+
+			/* Set the local address */
+			if (mp_usable_addresses[i].ss_family == AF_INET) {
+				laddr = &((struct sockaddr_in *)
+					&mp_usable_addresses[i])->sin_addr.s_addr;
+			}
+
+			/* Do not send any more joins from this local address */
+			mp->mp_advjoin_mask &= ~(1 << i);
+
+			/* Check for existing 5-tuple. Do not want to issue a
+			 * join across an existing address pair. */
+			inp = in_pcblookup(&V_tcbinfo, *((struct in_addr *)faddr),
+				fport, *((struct in_addr *)faddr), lport, INPLOOKUP_WLOCKPCB,
+				NULL);
+
+			if (inp) {
+				INP_WUNLOCK(inp);
+				printf("%s: abort joining existing tuple\n", __func__);
+				continue;
+			}
+
+			error = mp_join_do_connect(so, laddr, faddr, lport, fport);
+			if (error)
+			    break;
+		}
+	}
+
+    return error;
+}
+
+
+/*
+ * Find the next address to send a JOIN for, then
+ * assign this to addr and use when connecting the new
+ * subflow.
+ *
+ * XXXNJW: mostly for demo purposes
+ */
+static int
+mp_join_learned(struct mpcb *mp, struct socket *so)
+{
+	struct inpcb *inp;
+	int error = 0, found_foreign = 0;
+	void *laddr = NULL, *faddr = NULL;
+	u_int16_t lport, fport;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* find the first learned address that we haven't tried joining.
+	 * If we have learned about any remote addresses, attempt to connect
+	 * to these. Otherwise return. */
+	char buf[128];
+
+	if (mp->mp_lrnedjoin_mask) {
+		int i;
+		for (i = 0; i < mp->mp_added_address_count; i++) {
+			printf("%s: learned addr count %d\n", __func__, i);
+			inet_ntop(AF_INET, &(((struct sockaddr_in *)
+				&mp->mp_added_addresses[i])->sin_addr.s_addr),
+				buf, sizeof(buf));
+			printf("%s: current address %s\n", __func__, buf);
+			printf("%s: learned mask %u\n", __func__, mp->mp_lrnedjoin_mask);
+
+			/* continue if this index has sent join already */
+			if (!(mp->mp_lrnedjoin_mask & (1 << i)))
+				continue;
+
+			if (mp->mp_added_addresses[i].ss_family == AF_INET) {
+				faddr = &((struct sockaddr_in *)
+					&mp->mp_added_addresses[i])->sin_addr.s_addr;
+			}
+			found_foreign = 1;
+
+			inet_ntop(AF_INET, faddr, buf, sizeof(buf));
+				printf("%s: faddr now %s\n", __func__, buf);
+
+			/* Do not send any more joins to this added address. */
+			mp->mp_lrnedjoin_mask &= ~(1 << i);
+			break;
+		}
+
+	}
+
+	if (found_foreign == 0)
+       goto out;
+
+	lport = mp->mp_mppcb->mpp_lport;
+	fport = mp->mp_mppcb->mpp_fport;
+
+	/* try connect from default interface first */
+	laddr = &((struct sockaddr_in *)
+		&mp->mp_default_address)->sin_addr.s_addr;
+
+	inet_ntop(AF_INET, laddr, buf, sizeof(buf));
+	printf("%s: laddr now %s\n", __func__, buf);
+
+	error = mp_join_do_connect(so, laddr, faddr, lport, fport);
+	if (error)
+		return error;
+
+	/* Want to issue a connect from each of the local addresses
+	 * that are available */
+	int i;
+	for (i = 1; i < mp->mp_conn_address_count; i++) {
+		printf("%s: conn addr count %d\n", __func__, i);
+
+		/* Set the local address */
+		if (mp_usable_addresses[i].ss_family == AF_INET) {
+			laddr = &((struct sockaddr_in *)
+				&mp_usable_addresses[i])->sin_addr.s_addr;
+		}
+
+		if (mp_is_addr_default((struct sockaddr_in *)laddr, mp))
+			continue;
+
+		inet_ntop(AF_INET, laddr, buf, sizeof(buf));
+		printf("%s: laddr now %s\n", __func__, buf);
+
+		/* Check for existing 5-tuple. Do not want to issue a
+		 * join across an existing address pair. */
+		inp = in_pcblookup(&V_tcbinfo, *((struct in_addr *)faddr),
+			fport, *((struct in_addr *)faddr), lport, INPLOOKUP_WLOCKPCB, NULL);
+
+		if (inp) {
+			INP_WUNLOCK(inp);
+			printf("%s: abort joining existing tuple\n", __func__);
+			continue;
+		}
+
+		// now have the src and dst addresses, create a new socket and send
+		// the join.
+		if (mp_join_do_connect(so, laddr, faddr, lport, fport))
+			break;
+	}
+
+out:
+    return error;
+}
+
+static int
+mp_join_do_connect(struct socket *so, void* laddr, void* faddr,
+	u_int16_t lport, u_int16_t fport)
+{
+	int error = 0;
+	struct socket *sf_so;
+	struct mppcb *mpp;
+    struct inpcb *inp;
+    struct tcpcb *tp;
+	struct sockaddr nam;
+
+    mpp = sotomppcb(so);
+
+	/* creates a subflow ghost socket, inheriting state from the primary
+	 * socket (similar to sonewconn). */
+    error = mp_create_subflow_socket(so, &sf_so);
+    if (error)
+    	goto out;
+
+    /* Populate nam with foreign address details */
+	bcopy(faddr, &(((struct sockaddr_in *) &nam)->sin_addr.s_addr),
+		sizeof(struct in_addr));
+	((struct sockaddr_in *) &nam)->sin_port = fport;
+	nam.sa_len = sizeof(struct sockaddr_in);
+	nam.sa_family = AF_INET;
+
+	/* attach tcpcb and inpcb to the subflow socket */
+	error = tcp_attach(sf_so);
+	if (error)
+		goto out;
+
+	printf("%s: attached new subflow\n", __func__);
+
+	/* XXXNJW: Some setup of the inpcb. this is very hacky and must be
+	 * redone (for demo only) */
+	inp = sotoinpcb(sf_so);
+	INP_WLOCK(inp);
+
+	tp = intotcpcb(inp);
+	tp->t_mp_conn.remote_token = mpp->mpp_mpcb->remote_token;
+	tp->t_mp_conn.local_token = mpp->mpp_mpcb->local_token;
+
+	inp->inp_lport = lport;
+	bcopy(laddr, &inp->inp_laddr, sizeof(struct in_addr));
+
+	INP_HASH_WLOCK(&V_tcbinfo);
+	error = in_pcbinshash(inp);
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+
+	INP_WUNLOCK(inp);
+	if (error)
+		kdb_break();
+
+	/* Insert the new sufblow pcbs and gso into sf_list */
+	error = mp_insert_subflow(mpp->mpp_mpcb, sf_so);
+	if (error)
+		goto out;
+
+	/* Initiate a connection from the new subflow socket. */
+	error = (*(sf_so)->so_proto->pr_usrreqs->pru_connect)(sf_so, &nam,
+		curthread);
+	if (error)
+		kdb_break();
+
+out:
+    return error;
+}
+
+/* Called from mp_output (rather than the drop_task_handler) */
+void
+mp_drop_task(struct mpcb *mp, int acked)
+{
+	struct socket *so;
+	struct mbuf *mfree;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	so = mp->mp_mppcb->mpp_socket;
+	KASSERT(so != NULL, ("%s: so from context was NULL\n", __func__));
+
+	SOCKBUF_LOCK(&so->so_snd);
+	if (acked > sbavail(&so->so_snd)) {
+		//mp->snd_wnd -= sbavail(&so->so_snd);
+		/* Our dfin has been acked in this case. */
+		mfree = sbcut_locked(&so->so_snd,
+		    (int)sbavail(&so->so_snd));
+	} else {
+		mfree = sbcut_locked(&so->so_snd, acked);
+		//mp->snd_wnd -= acked;
+	}
+
+	/* NB: sowwakeup_locked() does an implicit unlock. */
+	sowwakeup_locked(so);
+	m_freem(mfree);
+	return;
+}
+
+static void
+mp_process_subflow_event(struct mpcb *mp, struct tcpcb *tp)
+{
+	if (tp->t_event_flags & SFE_MPESTABLISHED) {
+		tp->t_event_flags &= ~SFE_MPESTABLISHED;
+
+		/* The sequence numbers negotiated for MP connection */
+		mp->ds_idsn = tp->t_mp_conn.ds_idss;
+		mp->ds_idsr = tp->t_mp_conn.ds_idrs;
+
+		/* XXXNJW: a temp way to store some info for mp_joins */
+		mp_set_default_address(mp, tp->t_inpcb);
+		mp_set_connection_info(mp, tp);
+
+		/* Can now set the MP connection as established. */
+		mp_init_established(mp);
+	}
+
+	if (tp->t_event_flags & SFE_CONNECTED) {
+		tp->t_event_flags &= ~SFE_CONNECTED;
+		mp_sf_connected(mp, tp);
+		printf("%s: subflow %p on mp %p\n", __func__, tp, mp);
+		SDT_PROBE2(mptcp, session, mp_process_subflow_event, connected,
+			mp, tp);
+	}
+}
+
+/* During connection setup these values are held by mp_connection
+ * struct in the initial connecting subflow. Now store the tokens
+ * at the mp-level for use in JOINs, mpti struct */
+static void
+mp_set_connection_info(struct mpcb *mp, struct tcpcb *tp)
+{
+    mp->remote_key = tp->t_mp_conn.remote_key;
+    mp->remote_token = tp->t_mp_conn.remote_token;
+    mp->local_key = tp->t_mp_conn.local_key;
+    mp->local_token = tp->t_mp_conn.local_token;
+}
+
+static void
+mp_set_default_address(struct mpcb *mp, struct inpcb *inp)
+{
+	void * addr = NULL;
+	int addr_len = 0;
+
+	/* Default local and foreign ports for the connection */
+	mp->mp_mppcb->mpp_lport = inp->inp_lport;
+	mp->mp_mppcb->mpp_fport = inp->inp_fport;
+
+	/* Hard-coded for IPv4 for now */
+	addr_len = 4;
+	addr = &((struct sockaddr_in *) &mp->mp_default_address)->sin_addr.s_addr;
+	bcopy(&inp->inp_laddr.s_addr, addr, addr_len);
+	mp->mp_default_address.ss_family = AF_INET;
+	mp->mp_default_address.ss_len = sizeof(struct sockaddr_in);
+
+	/* Default foreign address */
+	addr = &((struct sockaddr_in *) &mp->mp_foreign_address)->sin_addr.s_addr;
+	bcopy(&inp->inp_faddr.s_addr, addr, addr_len);
+	mp->mp_foreign_address.ss_family = AF_INET;
+	mp->mp_foreign_address.ss_len = sizeof(struct sockaddr_in);
+
+}
+
+/* XXXNJW: A temp way to clean out subflows.
+ * Sort of a garbage collection for subflows that
+ * have been tcp_closed but were never freed (due
+ * to pru_close not being called by waiting
+ * thread). */
+void
+mp_close_subflow_task_handler(void *context, int pending)
+{
+	struct mpcb *mp;
+	struct mppcb *mpp;
+
+	mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+
+	MPP_LOCK(mpp);
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: so %p mpp %p mp %p\n", __func__, mpp->mpp_socket, mpp,
+			mpp->mpp_mpcb);
+		return;
+	}
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+
+	mp->mp_sf_close_pending = 0;
+	mp_close_subflow(mp);
+
+	MPP_UNLOCK(mp->mp_mppcb);
+}
+
+/* XXNJW: called under MPP locks. This is currently used
+ * only in cases where a subflow is not able to free itself
+ * (i.e. doesn't go through a disconnect, taking SS_PROTOREF).
+ * It is a temporary solution - need to go through and think
+ * about the closing states of subflows in more depth.
+ *
+ * The inp is already dropped, socket should be disconnected.
+ * SS_PROTOREF and INP_SOCKREF should not be set in this case,
+ * as we want sorele to ultimately call detach and free. */
+static int
+mp_close_subflow(struct mpcb *mp)
+{
+	struct sf_handle *sfhandle;
+	struct inpcb *inp;
+    struct socket *so;
+
+    TAILQ_FOREACH(sfhandle, &mp->sf_list, next_sf_handle) {
+		if (sfhandle->sf_flags & SFHS_MPENDED)
+			continue;
+
+		so = sfhandle->sf_so;
+		KASSERT(so != NULL, ("%s: so == NULL", __func__));
+
+		inp = sotoinpcb(so);
+
+		/* As we've got mutltiple subflows closing, and aren't
+		 * freeing the handles (and not always setting ended
+		 * on the handle...) a cheap hack to stop panics */
+		if (inp == NULL) {
+			sfhandle->sf_flags |= SFHS_MPENDED;
+			continue;
+		}
+
+//		KASSERT(inp != NULL, ("%s: inp NULL\n", __func__));
+
+		INP_WLOCK(inp);
+
+//		printf("%s: inp flags %d\n", __func__, inp->inp_flags);
+//		printf("%s: sfh flags %d\n", __func__, sfhandle->sf_flags);
+
+		/* Timewait subflows will dealloc themselves when
+		 * the timer expires */
+		if (inp->inp_flags & INP_TIMEWAIT) {
+			INP_WUNLOCK(inp);
+			continue;
+		}
+
+		/* Only interested in subflows that can be freed now. */
+		if (!(inp->inp_flags & INP_DROPPED)) {
+			INP_WUNLOCK(inp);
+			continue;
+		}
+
+		/* XXXNJW: temp - to make sure we don't try to dereference the
+		 * inp from this point onwards.
+		 *
+		 * The inp won't be available after this. */
+		sfhandle->sf_flags |= SFHS_MPENDED;
+		sfhandle->sf_so = NULL;
+
+		/* also remove this sfhandle from the list and free? */
+
+		/* soisdisconnected should have been called if we
+		 * are calling a close from here? */
+	//	KASSERT(so->so_state == SS_ISDISCONNECTED,
+	//		("%s: socket !disconnected\n", __func__));
+		printf("%s: so_state %d\n", __func__, so->so_state);
+		INP_WUNLOCK(inp);
+
+		/* need to remove, close subflow.
+		 * XXXNJW:*/
+		(*so->so_proto->pr_usrreqs->pru_close)(so);
+		if (!(so->so_state & SS_PROTOREF)) {
+			printf("%s: free and release subflow socket\n", __func__);
+			sfhandle->sf_so = NULL;
+
+			/* sofree on the subflow socket */
+			mp_subflow_release_socket(so);
+
+			/* Decrement subflow count. this will result in a
+			 * call to mp_close (as we only have a single sublow
+			 * in non mp_connected sessions. */
+			KASSERT(mp != NULL, ("%s: mp NULL\n", __func__));
+			if (mp_detach_subflow_locked(mp))
+				return 1;
+		}
+	}
+
+    return 0;
+}
+
+
+
+
+
+//static void
+//mp_close_subflow(struct inpcb *inp)
+//{
+//	struct socket *so;
+//
+//
+//	KASSERT(inp != NULL, ("%s: inp == NULL", __func__));
+//
+//	so = inp->inp_socket;
+//	KASSERT(so != NULL, ("%s: so == NULL", __func__));
+//
+//	/* soisdisconnected should have been called if we
+//	 * are calling a close from here? */
+////	KASSERT(so->so_state == SS_ISDISCONNECTED,
+////		("%s: socket !disconnected\n", __func__));
+//    printf("%s: so_state %d\n", __func__, so->so_state);
+//
+//	INP_WUNLOCK(inp);
+//
+//	/* need to remove, close subflow. */
+//	(*so->so_proto->pr_usrreqs->pru_close)(so);
+//	mp_subflow_release_socket(so);
+//}
+
+
+//static void
+//mp_process_event(struct mpcb *mp)
+//{
+//	mp->mp_sf_event_pending = 0;
+//
+//}
+
+//void
+//mp_trigger_rexmit(struct mpcb *mp) {
+//	mpp_pcbref(mp->mp_mppcb);
+//	taskqueue_enqueue(taskqueue_swi, &mp->rexmit_task);
+////	if (atomic_cmpset_int(&mp->mp_rexmit_pending, 0, 1))
+////		mpp_pcbref(mp->mp_mppcb);
+//}
+
+
+///*
+// * Find the first subflow that isn't in subflow-level rexmit and do data-level
+// * rexmit on this flow (just call tcp_output, the ds_map code worries about
+// * XXXNJW: or perhaps whichever subflow has cwnd > 1MSS?
+// */
+//void
+//mp_rexmit_task_handler(void *context, int pending)
+//{
+//	struct mpcb *mp;
+//	struct mppcb *mpp;
+//	struct sf_handle *sfhandle;
+//    struct inpcb *inp;
+//    struct tcpcb *tp;
+//
+//	mpp = (struct mppcb *)context;
+//	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+//
+//	mp = mpp->mpp_mpcb;
+//	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+//
+//	MPP_LOCK(mpp);
+//	if (mpp_pcbrele(mpp))
+//		kdb_break();
+//
+//	mp->mp_rexmit_pending = 0;
+//
+//	TAILQ_FOREACH(sfhandle, &mp->sf_list, next_sf_handle) {
+//		inp = sotoinpcb(sfhandle->sf_so);
+//		KASSERT(inp != NULL, ("%s: inp NULL\n", __func__));
+//		INP_WLOCK(inp);
+//
+//		tp = intotcpcb(inp);
+//		if (!(inp->inp_flags & INP_DROPPED) && tp->t_rxtshift == 0
+//		    && tp->t_state == TCPS_ESTABLISHED) {
+//			break;
+//		}
+//		INP_WUNLOCK(inp);
+//	}
+//	MPP_UNLOCK(mpp);
+//
+//	if (sfhandle != NULL) {
+//		tcp_output(tp);
+//		INP_WUNLOCK(inp);
+//	}
+//
+//	if (tp == NULL)
+//		mp_debug(MPSESSION, 1, 0, "%s: all subflows in rexmit\n", __func__);
+//}
+
+
+/*
+ * Move ds_snd_una forward and schedule drop of bytes acked. if ack'ing
+ * past snd_max, then this is an ACK of a DFIN, so we can begin to close
+ * all the subflows, and the connection.
+ *
+ * If the send window is partially ACKd, then we reset the data-level RTO
+ * timer. If the window is fully ACKd, the timer is stopped.
+ *
+ */
+int
+mp_data_ack(struct mpcb *mp, uint64_t data_ack_num) {
+    struct socket *so = mp->mp_mppcb->mpp_socket;
+    struct mbuf *mfree;
+    int acked = 0, needoutput = 0;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    mp_debug(MPSESSION, 4, 0, "processing DACK %ju (%u), ds-una is %ju\n",
+			data_ack_num, (uint32_t)data_ack_num, mp->ds_snd_una);
+
+//    printf("%s: dack %ju una %ju max %ju\n", __func__, data_ack_num,
+//        mp->ds_snd_una, mp->ds_snd_max);
+
+    acked = data_ack_num - mp->ds_snd_una;
+    if(acked) {
+    	/* XXXNJW: Update mp_rxtcur to base value. Not taking into
+    	 * account SRTT of the subflows etc. Also need a less silly
+    	 * method of setting these values (i.e. put them in a macro
+    	 * or something. */
+    	mp->mp_rxtshift = 0;
+    	mp->mp_rxtcur = MPTCPTV_RTOBASE;
+
+    	/* Check for DFIN being acked in mp_drop_task */
+    	//mp_drop_task(mp, acked);
+
+    	if (data_ack_num == mp->ds_snd_max)
+    		needoutput = 1;
+
+//    	printf("%s: acked %d\n", __func__, acked);
+
+    	SOCKBUF_LOCK(&so->so_snd);
+		if (acked > sbavail(&so->so_snd)) {
+			/* Our dfin has been acked in this case. */
+			printf("%s: finisacked %ju snd_nxt %ju snd_una %ju\n", __func__,
+			    data_ack_num, mp->ds_snd_nxt, mp->ds_snd_una);
+			printf("%s: finisacked %u snd_nxt %u snd_una %u\n", __func__,
+				(uint32_t)data_ack_num, (uint32_t)mp->ds_snd_nxt,
+				(uint32_t)mp->ds_snd_una);
+			printf("%s: acked %d\n", __func__, acked);
+
+			//mp->snd_wnd -= sbavail(&so->so_snd);
+			mfree = sbcut_locked(&so->so_snd,
+				(int)sbavail(&so->so_snd));
+		} else {
+			mfree = sbcut_locked(&so->so_snd, acked);
+			//mp->snd_wnd -= acked;
+		}
+
+		/* NB: sowwakeup_locked() does an implicit unlock. */
+		sowwakeup_locked(so);
+		m_freem(mfree);
+
+    	mp->ds_snd_una = data_ack_num;
+    	if (DSEQ_LT(mp->ds_snd_nxt, mp->ds_snd_una))
+    		mp->ds_snd_nxt = mp->ds_snd_una;
+
+    }
+
+    /* MP-level rexmit timer */
+    if (mp->mp_connected) {
+		if (data_ack_num == mp->ds_snd_max) {
+			if (mp_timer_active(mp, MPT_REXMT))
+				mp_timer_activate(mp, MPT_REXMT, 0);
+		} else if (DSEQ_LT(data_ack_num, mp->ds_snd_max)) {
+				mp_timer_activate(mp, MPT_REXMT, mp->mp_rxtcur);
+		} else {/* if (!in persist)? */ }
+    }
+
+    return (needoutput);
+}
+
+
+
+//static void
+//mp_rcvd_dfin_ack(struct mpcb *mp)
+//{
+//	printf("%s: entered with state %d\n", __func__, mp->mp_state);
+//
+//	struct socket *so = mp->mp_mppcb->mpp_socket;
+//    int do_output = 0;
+//
+//	MPP_LOCK_ASSERT(mp->mp_mppcb);
+//
+//    switch (mp->mp_state) {
+//	case MPS_M_FIN_WAIT_1:
+//		if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
+//			soisdisconnected(so);
+//		mp->mp_state = MPS_M_FIN_WAIT_2;
+//		break;
+//
+//	case MPS_M_CLOSING:
+//		mp->mp_state = MPS_M_TIME_WAIT;
+//		do_output = 1;
+//		break;
+//
+//	case MPS_M_LAST_ACK:
+//		mp->mp_state = MPS_M_CLOSED;
+//		do_output = 1;
+//	}
+//
+//    /* Moving to time_wait or closed should trigger close
+//     * on all subflows */
+//    if (do_output) {
+//    	mpp_pcbref(mp->mp_mppcb);
+//        taskqueue_enqueue(taskqueue_swi, &mp->mp_output_task);
+//    }
+//
+//    printf("%s: exit with state %d\n", __func__, mp->mp_state);
+//}
+
+//static void
+//mp_rcvd_dfin(struct mpcb *mp)
+//{
+//	printf("%s: entered with state %d\n", __func__, mp->mp_state);
+//
+//	MPP_LOCK_ASSERT(mp->mp_mppcb);
+//
+//    struct socket *so = mp->mp_mppcb->mpp_socket;
+//    int need_output = 0;
+//    socantrcvmore(so);
+//
+//	switch (mp->mp_state) {
+//	case MPS_M_ESTABLISHED:
+//		mp->mp_state = MPS_M_CLOSE_WAIT;
+//		break;
+//
+//	case MPS_M_FIN_WAIT_1:
+//		mp->mp_state = MPS_M_CLOSING;
+////		if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
+////			soisdisconnected(so);
+////			mp_disconnect_all_subflows(mp); // let mp_output/usrreq do this?
+////            /* Some kind of FW2 timer so we don't wait
+////             * around in FW2 for a dfin ack? */
+////		} else /* Should send a data-ack of the DFIN we've got */
+//			need_output = 1;
+//		break;
+//
+//	case MPS_M_FIN_WAIT_2:
+//		mp->mp_state = MPS_M_TIME_WAIT;
+//		/* Moving to time_wait should trigger close on all subflow.
+//		 * For now this is done by calling into mp_output, where
+//		 * we check the mp_state and call a disconnect. */
+//		need_output = 1;
+//		break;
+//	}
+//
+//	/* Schedule calling of mp_output if we need to respond to
+//	 * this */
+////	if (need_output && mp->output_task_pending == 0) {
+////		atomic_add_int(&mp->output_task_pending, 1);
+//	if (need_output) {
+//		mpp_pcbref(mp->mp_mppcb);
+//		taskqueue_enqueue(taskqueue_swi, &mp->mp_output_task);
+//	}
+////	}
+//
+//    printf("%s: exit with state %d\n", __func__, mp->mp_state);
+//}
+
+
+
+/*
+ * Accesses socket buffer on behalf on a subflow and returns a mapping
+ * into the send buffer than the subflow will transmit from.
+ *
+ * XXXNJW: currently restricted to returning a map of size 1420 (this allows
+ * space for header and options to be added while staying under the typical
+ * ethernet MSS)
+ */
+static struct ds_map*
+mp_get_map(struct mpcb *mp, int length) {
+	struct ds_map *map = NULL;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* XXXNJW removed the code searching for maps that need rexmit. need
+     * to put back in later, or maybe rethink how retransmission work (maybe
+     * just make a new map altogether? though this will mess up the reference
+     * counting and freeing the socket buffer) */
+	if(length) {
+		map = malloc(sizeof(struct ds_map), M_DSSMAP, M_NOWAIT|M_ZERO);
+		if (map) {
+			map->ds_map_start = mp->ds_snd_nxt;
+			map->ds_map_len = map->ds_map_remain = length;
+		}
+	}
+
+	return(map);
+}
+
+
+struct ds_map *
+mp_find_dsmap(struct tcpcb *tp, tcp_seq	seqnum)
+{
+	struct ds_map *map = NULL;
+	tcp_seq sf_seq_end;
+
+	INP_WLOCK_ASSERT(tp->t_inpcb);
+
+	/* If there aren't any maps, return */
+	if(TAILQ_EMPTY(&tp->t_send_maps.dsmap_list))
+		return NULL;
+
+	/* Performing wrap detection on map lookups based on sequence number.
+	 * If (sf_seq_end = sf_start + length) wraps, sf_seq_end will be less
+	 * than sf_seq_start. We detect this with: seqnum <= sf_seq_end (seqnum
+	 * is in map) && sf_seq_end < map->sf_seq_start
+	 */
+	TAILQ_FOREACH(map, &tp->t_send_maps.dsmap_list, sf_ds_map_next) {
+		sf_seq_end =  map->sf_seq_start + map->ds_map_len;// - 1;
+
+		/* reached end of this map list, return a NULL map, as mp_output
+		 * should be allocating new maps for this subflow */
+		if (seqnum == sf_seq_end)
+			continue;
+
+		/* match the first map which covers the passed-in sequence num.
+		 * includes checks for wrapped maps. */
+		if ((seqnum >= map->sf_seq_start && sf_seq_end > map->sf_seq_start
+			&& seqnum < sf_seq_end) ||
+			(sf_seq_end < map->sf_seq_start && seqnum < map->sf_seq_start
+			&& seqnum < sf_seq_end) ||
+			(sf_seq_end < map->sf_seq_start && seqnum >= map->sf_seq_start
+			&& seqnum <= UINT_MAX)) {
+				mp_debug(DSMAP, 4, 0, "%s  sequence num: %u"
+					"starts at %u, ends %u\n", __func__,
+					seqnum, map->sf_seq_start, sf_seq_end);
+				break;
+		}
+	}
+
+	return (map);
+}
+
+
+/*
+ * Attempt to close a MP control block, marking it as dropped, and freeing
+ * the socket if we hold the only reference.
+ */
+struct mpcb *
+mp_close(struct mpcb *mp)
+{
+	printf("%s: entered\n", __func__);
+
+	struct mppcb *mpp = mp->mp_mppcb;
+	struct socket *so;
+
+	MPP_LOCK_ASSERT(mpp);
+
+	mpp_pcbdrop(mpp);
+	so = mpp->mpp_socket;
+	KASSERT(so != NULL, ("%s: mpp_socket NULL\n", __func__));
+
+	soisdisconnected(so);
+
+	/* Would expect MPP_SOCKREF to be set in all cases except for
+	 * a usr_close on an un-synchronised connection. */
+	if (mpp->mpp_flags & MPP_SOCKREF) {
+		KASSERT(so->so_state & SS_PROTOREF,
+		    ("mp_close: !SS_PROTOREF"));
+		mpp->mpp_flags &= ~MPP_SOCKREF;
+		MPP_UNLOCK(mpp);
+		ACCEPT_LOCK();
+		SOCK_LOCK(so);
+		so->so_state &= ~SS_PROTOREF;
+		printf("%s: call sofree\n", __func__);
+		sofree(so);
+		return (NULL);
+	}
+	return (mp);
+}
+
+/* XXXNJW: Temp, to improve */
+void
+mp_close_all_subflows(struct mpcb *mp)
+{
+	struct sf_handle *sf_h = NULL;
+	struct socket *so;
+	int error = 0;
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* XXXNJW: temp hack
+     * The sf handles are freed when the mpcb is released. The count is
+     * decremented when the subflow is detached from the subflow socket.
+     * This is a bit of a messy copy of soclose, without the processing
+     * for sockets with state SO_ACCEPTCONN */
+	TAILQ_FOREACH(sf_h, &mp->sf_list, next_sf_handle) {
+		if((sf_h->sf_flags & (SFHS_MPENDED | SFHS_DISCONNECTING)) == 0) {
+			so = sf_h->sf_so;
+			KASSERT(so != NULL, ("%s: subflow so NULL\n", __func__));
+			printf("%s: disconnecting subflow tp %p\n", __func__,
+			    sototcpcb(sf_h->sf_so));
+		    error = sodisconnect(sf_h->sf_so);
+            if (error == 0) {
+            	(*so->so_proto->pr_usrreqs->pru_close)(sf_h->sf_so);
+            	ACCEPT_LOCK();
+            	SOCK_LOCK(sf_h->sf_so);
+            	sorele(sf_h->sf_so);
+            }
+		    sf_h->sf_flags |= SFHS_DISCONNECTING;
+		}
+	}
+}
+
+void
+mp_reset_all_subflows(struct mpcb *mp)
+{
+	printf("%s\n", __func__);
+
+	struct sf_handle *sf_h = NULL;
+	struct inpcb *inp;
+	struct tcpcb *tp;
+	struct socket *so;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	INP_INFO_WLOCK(&V_tcbinfo);
+
+	/* XXXNJW: temp hack
+	 * The sf handles are freed when the mpcb is released. The count is
+	 * decremented when the subflow is detached from the subflow socket.
+	 * This is a bit of a messy copy of soclose, without the processing
+	 * for sockets with state SO_ACCEPTCONN */
+	TAILQ_FOREACH(sf_h, &mp->sf_list, next_sf_handle) {
+		printf("%s: sf_flags %d\n", __func__, sf_h->sf_flags);
+		if((sf_h->sf_flags & SFHS_MPENDED) == 0) {
+			so = sf_h->sf_so;
+			KASSERT(so != NULL, ("%s: subflow so NULL\n", __func__));
+			printf("%s: RST subflow at handle %p\n", __func__, sf_h);
+			inp = sotoinpcb(sf_h->sf_so);
+			INP_WLOCK(inp);
+
+			if (inp->inp_flags & INP_DROPPED) { /* already dropped */
+				INP_WUNLOCK(inp);
+				continue;
+			}
+
+			tp = intotcpcb(inp);
+			tp->t_sf_flags = 0; /* No DACKs, etc to be added */
+			tp->t_sf_state = SFS_MP_DISCONNECTED;
+			tp = tcp_drop(tp, tp->t_softerror ?
+		        tp->t_softerror : ETIMEDOUT); /* Sends RST, calls tcp_close */
+
+			/* tp should not be NULL in here, as INP_SOCKREF is not set.
+			 * The INP and subflow socket will be freed from the call to
+			 * soclose. */
+			KASSERT(tp != NULL, ("%s: tp null after tcp_drop\n", __func__));
+			if (tp != NULL)
+			    INP_WUNLOCK(inp);
+
+			sf_h->sf_flags |= SFHS_MPENDED;
+		}
+	}
+	INP_INFO_WUNLOCK(&V_tcbinfo);
+
+}
+
+void
+mp_subflow_freehandle(struct mpcb *mp, struct sf_handle *sf)
+{
+	printf("%s\n", __func__);
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+	/* remove subflows with NULL tps */
+	TAILQ_REMOVE(&mp->sf_list, sf, next_sf_handle);
+	free(sf, M_SFHANDLE);
+}
+
+void
+mp_subflow_release_socket(struct socket *so)
+{
+	ACCEPT_LOCK();
+	SOCK_LOCK(so);
+	sorele(so);
+}
+
+void
+mp_subflow_detached_task_handler(void *context, int pending)
+{
+	struct mppcb *mpp;
+    struct mpcb *mp;
+
+	mpp = (struct mppcb *)context;
+	KASSERT(mpp != NULL, ("%s: mpp from context was NULL\n", __func__));
+	MPP_LOCK(mpp);
+
+	printf("%s: mpp_locked. pending %d\n", __func__, pending);
+
+	mp = mpp->mpp_mpcb;
+	KASSERT(mp != NULL, ("%s: mp from context was NULL\n", __func__));
+    mp->mp_sf_detach_pending = 0;
+
+	if (mpp->mpp_refcount < 2)
+	    printf("%s: mpp ref count %d\n", __func__, mpp->mpp_refcount);
+
+	if (mpp_pcbrele(mpp)) {
+		printf("%s: so %p mpp %p mp %p\n", __func__, mpp->mpp_socket, mpp,
+			mpp->mpp_mpcb);
+		return;
+	}
+
+    mp_subflow_detached(mpp->mpp_mpcb, pending);
+
+}
+
+int
+mp_detach_subflow_locked(struct mpcb *mp)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+	printf("%s: entered\n", __func__);
+	return mp_subflow_detached(mp, 1);
+}
+
+/* XXXNJW: tcp_discardcb has been called, so remove the subflow from the list
+ * as it is not longer useful. A time_wait subflow will remove itself and does
+ * not need access to the mpcb.
+ *
+ * Currently cleans up any NULL tp blocks.
+ * Since this is done asynchronously, is it possible the memory pointed to by
+ * the tp could be assigned to a new tp in the meantime? */
+static int
+mp_subflow_detached(struct mpcb *mp, int count)
+{
+    int unlocked = 0;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    printf("%s: mp %p sf cnt %d\n", __func__, mp, mp->subflow_cnt);
+	KASSERT(count <= mp->subflow_cnt,
+	    ("%s: detach count exceeds subflow count\n", __func__));
+
+	for (int i = 0; i < count; i++) {
+		KASSERT(mp->subflow_cnt > 0, ("%s: subflow count < 0\n", __func__));
+		if (--mp->subflow_cnt == 0) {
+			/* Returns > 0 if the mp has been freed */
+			unlocked = mp_detached_last_subflow(mp);
+		}
+	}
+
+	return unlocked;
+}
+
+static int
+mp_detached_last_subflow(struct mpcb *mp)
+{
+	int unlocked = 0;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	// need to think about half-synchronised connections,
+	// regular tcp connections and so forth.
+
+	/* If the mp-level is already disconnected, or never
+	 * connected we can just close and free the PCBs now.
+	 * Otherwise start a timer in case a subflow re-appears. */
+	if (mp->mp_mppcb->mpp_flags & MPP_TIMEWAIT) {
+        /* Wait for timewait to end before closing. */
+		printf("%s: last subflow, in M_TW mpp %p\n",
+		    __func__, mp->mp_mppcb);
+		if (!mp_timer_active(mp, MPT_TIMEOUT))
+			mp_timer_activate(mp, MPT_TIMEOUT, mp_timeout());
+    } else if (mp->mp_state == MPS_M_CLOSED) {
+    	printf("%s: last subflow, in M_CLOSED mpp %p\n",
+    	    __func__, mp->mp_mppcb);
+    	/* Unlike TW case, there should be a socket still
+    	 * attached to the mpp at this point, so should
+    	 * be okay to call mp_close */
+		if (!mp_close(mp))
+			unlocked = 1;
+	} else if (!mp_timer_active(mp, MPT_TIMEOUT))
+	    mp_timer_activate(mp, MPT_TIMEOUT, mp_timeout());
+
+	printf("%s: unlocked %d\n", __func__, unlocked);
+	return unlocked;
+}
+
+/* Just remove all the subflow handles that might have been allocated.
+ * XXXNJW: Should really make sure all the subflows have been dropped and
+ * dealloced so that there aren't any left floating around with state. */
+void
+mp_sf_flush(struct mpcb *mp)
+{
+	struct sf_handle *sf;
+	while ((sf = TAILQ_FIRST(&mp->sf_list)) != NULL) {
+		TAILQ_REMOVE(&mp->sf_list, sf, next_sf_handle);
+		free(sf, M_SFHANDLE);
+	}
+}
+
+/* XXXNJW: currently not freeing any recorded options during a
+ * connection, and are just removing them all at the end. */
+void
+mp_mpsopt_flush(struct mpcb *mp)
+{
+	struct mp_sopt *mpsopt;
+	while ((mpsopt = TAILQ_FIRST(&mp->mp_sopt_list)) != NULL) {
+		TAILQ_REMOVE(&mp->mp_sopt_list, mpsopt, next_mp_sopt);
+		free(mpsopt, M_MPSOPT);
+	}
+}
+
+
+/* This is not actually allocating a proper "socket", Just want the struct
+ * to be initialised and then copy some state from the actual MP socket. The
+ * subflows will use/set flags as needed to track their own state. Eventually
+ * don't want to allocate an entire socket.
+ */
+struct socket *
+mp_allocghostsocket(struct socket *so) {
+	struct socket *sf_gso = NULL;
+
+	KASSERT(so != NULL, ("mp_allocghostsocket: so == NULL"));
+
+	sf_gso = gsoalloc(so->so_vnet);
+	if (sf_gso == NULL)
+		return (NULL);
+
+	knlist_init_mtx(&sf_gso->so_rcv.sb_sel.si_note, SOCKBUF_MTX(&sf_gso->so_rcv));
+	knlist_init_mtx(&sf_gso->so_snd.sb_sel.si_note, SOCKBUF_MTX(&sf_gso->so_snd));
+
+	/* Inherit state from the connection socket (though don't set so_head) */
+	sf_gso->so_head = NULL;
+	sf_gso->so_options = so->so_options &~ SO_ACCEPTCONN;
+	sf_gso->so_state = so->so_state;
+	sf_gso->so_linger = so->so_linger;
+	sf_gso->so_state = so->so_state | SS_NOFDREF;
+	sf_gso->so_fibnum = so->so_fibnum;
+	sf_gso->so_cred = crhold(so->so_cred);
+    sf_gso->so_count = 1;
+
+	/* A protosw struct for subflows.
+	 * Subflows can use standard tcp proto hooks (e.g. tcp_usr_detach).
+	 * Need to do this in a nicer way. */
+	sf_gso->so_proto = &sf_protosw;
+
+	return sf_gso;
+}
+
+void
+mp_sftimewait(struct socket *sf_gso)
+{
+//	struct inpcb *sf_inp;
+//	struct inpcb *inp;
+//	struct mpcb *mp;
+////	struct socket *so;
+//
+//	SOCK_LOCK_ASSERT(sf_gso);
+//	sf_inp = sotoinpcb(sf_gso);
+//	KASSERT(sf_inp != NULL, ("%s: inp NULL\n", __func__));
+//
+//	mp = intompcb(sf_inp);
+//	KASSERT(mp != NULL, ("%s: mp NULL\n", __func__));
+//
+//	inp = mp->mp_inpcb;
+//	INP_WLOCK(inp);
+//	inp->inp_flags |= INP_TIMEWAIT;
+//	INP_WUNLOCK(inp);
+}
+
+
+int
+mp_create_subflow_implicit(struct mpcb *mp, struct socket *so, struct ip *ip,
+	struct tcphdr *th)
+{
+	int error;
+	struct socket *sf_so;
+    struct inpcb *inp;
+    struct tcpcb *tp;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* creates a subflow ghost socket, inheriting state from the primary
+	 * socket (similar to sonewconn). */
+	error = mp_create_subflow_socket(so, &sf_so);
+	if (error)
+        goto out;
+
+	KASSERT(sf_so != NULL, ("%s: subflow socket NULL", __func__));
+	sf_so->so_state &= (SS_NOFDREF | SS_NBIO | SS_ASYNC);
+
+	/* attach tcpcb and inpcb to the subflow socket */
+	error = tcp_attach(sf_so);
+	if (error)
+		goto out;
+
+    inp = sotoinpcb(sf_so);
+    KASSERT(inp != NULL, ("%s: subflow inp NULL", __func__));
+    INP_WLOCK(inp);
+
+    inp->inp_lport = th->th_dport;
+    inp->inp_fport = th->th_sport;
+    inp->inp_faddr.s_addr = ip->ip_src.s_addr;
+    inp->inp_laddr.s_addr = ip->ip_dst.s_addr;
+
+    INP_HASH_WLOCK(&V_tcbinfo);
+    in_pcbinshash(inp);
+    INP_HASH_WUNLOCK(&V_tcbinfo);
+
+    tp = intotcpcb(inp);
+
+	SOCK_LOCK(sf_so);
+	error = solisten_proto_check(sf_so);
+	if (error == 0) {
+		tp->t_state = TCPS_LISTEN;
+		solisten_proto(sf_so, 2);
+	}
+	SOCK_UNLOCK(sf_so);
+
+	if (error) {
+		printf("%s: error %d subflow socket state %d\n", __func__, error,
+		    sf_so->so_state);
+		kdb_break();
+	}
+
+    soisconnecting(sf_so);
+	tp->t_sf_flags |= SFF_PASSIVE_JOIN;
+	tp->iss = tcp_new_isn(tp);
+    tp->irs = th->th_seq;
+    tp->t_mp_conn.local_key = mp->local_key;
+    tp->t_mp_conn.remote_key = mp->remote_key;
+    INP_WUNLOCK(inp);
+
+	/* Insert the new sufblow pcbs and gso into sf_list (takes MP_LOCK) */
+	error = mp_insert_subflow(mp, sf_so);
+out:
+   MPP_UNLOCK(mp->mp_mppcb);
+   return error;
+}
+
+
+/*
+ * Add a subflow to the provided mp connection. Allocate a new sunflow handle
+ * (tp, inp, g_so) and insert into the list of subflows.
+ */
+int
+mp_insert_subflow(struct mpcb *mp, struct socket *sf_so)
+{
+	struct sf_handle *new_sf = NULL;
+    struct tcpcb *tp;
+    int error = 0;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	new_sf = malloc(sizeof(struct sf_handle), M_SFHANDLE, M_NOWAIT);
+	if (new_sf == NULL) {
+		error = ENOMEM;
+        goto out;
+	}
+
+	new_sf->sf_so = sf_so;
+	new_sf->sf_flags = 0;
+	tp = intotcpcb(sotoinpcb(sf_so));
+
+	/* Set the mpcb pointer */
+	tp->t_mpcb = mp;
+
+	TAILQ_INSERT_TAIL(&mp->sf_list, new_sf, next_sf_handle);
+	if (mp->subflow_cnt == 0) {
+		tp->t_sf_flags |= SFF_FIRSTSUBFLOW;
+		tp->t_sf_flags |= SFF_INFINITEMAP;
+		tp->t_sf_flags |= SFF_SEND_MPCAPABLE;
+	}
+	tp->t_addrid = mp->subflow_cnt++;
+
+out:
+	return (error);
+}
+
+int
+mp_attach_subflow(struct socket *so)
+{
+	 return tcp_attach(so);
+}
+
+
+/* Append the just received mbuf to the queue of segments
+ * to be processed by mp_input. Don't need to allocate
+ * anything. Assumes that there is at least one packet
+ * in the list, inserts at the end. */
+void
+mp_appendpkt(struct mbuf *mb, struct mbuf *m_ptr)
+{
+    struct mbuf *mq;
+
+    KASSERT(mb != NULL, ("%s: mbuf NULL\n", __func__));
+
+    mq = mb;
+    while (mq->m_nextpkt) {
+	    mq = mq->m_nextpkt;
+    }
+    mq->m_nextpkt = m_ptr;
+
+    return;
+}
+
+void
+mp_mbuf_enqueue(struct mpcb *mp, struct mbuf *m)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+    KASSERT(mp != NULL, ("%s: mp NULL", __func__));
+    KASSERT(m != NULL, ("%s: m NULL", __func__));
+
+	if (mp->mp_input_segq == NULL)
+		mp->mp_input_segq = m;
+	else
+		mp_appendpkt(mp->mp_input_segq, m);
+
+
+}
+
+void
+mp_reass_flush(struct mpcb *mp)
+{
+	struct mbuf *m;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	while ((m = mp->mp_segq) != NULL) {
+		mp->mp_segq = m->m_nextpkt;
+		mp->mp_segqlen -= m->m_pkthdr.len;
+		m_freem(m);
+	}
+
+	KASSERT((mp->mp_segqlen == 0),
+	    ("MPTCP reass queue %p length is %d instead of 0 after flush.",
+	    mp, mp->mp_segqlen));
+}
+
+
+int
+mp_reass(struct mpcb *mp, struct mbuf *m)
+{
+	struct mbuf *mq, *mpre;
+	struct socket *so = mp->mp_mppcb->mpp_socket;
+	int flags = 0, wakeup = 0, todrop;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	M_ASSERTPKTHDR(m);
+    KASSERT(m_tag_locate(m, PACKET_COOKIE_MPTCP, PACKET_TAG_DSN, NULL) != NULL,
+        ("%s: no dsn tag on segment\n", __func__));
+
+//	printf("%s: packetlen %d\n", __func__, m->m_pkthdr.len);
+//    printf("%s: ds_rcv_nxt: %u mdsn %u\n", __func__,
+//        (uint32_t) mp->ds_rcv_nxt, (uint32_t) M_MPTCPDSN(m));
+//    printf("%s: ds_rcv_nxt: %ju mdsn %ju\n", __func__,
+//		mp->ds_rcv_nxt, M_MPTCPDSN(m));
+    if (M_MPTCPDSNFLAGS(m) & MP_DFIN)
+		printf("%s: inserting dfin seg %u ds_rcv_nxt: %u\n",__func__,
+		    (uint32_t) mp->ds_rcv_nxt, (uint32_t) M_MPTCPDSN(m));
+
+    /* Trim overlapping at data level, or drop if duplicate */
+    todrop = mp->ds_rcv_nxt - M_MPTCPDSN(m);
+    if (todrop > 0) {
+//    	printf("%s: dup segment %u len %d todrop %d \n", __func__,
+//    		(uint32_t) M_MPTCPDSN(m), m->m_pkthdr.len, todrop);
+    	/* Partially duplicated segment. Trim until
+		 * we reach the new data. Otherwise a complete
+		 * duplicate that can be freed. goto present
+		 * to read in any queued data. */
+    	if (todrop < m->m_pkthdr.len) {
+			M_MPTCPDSN(m) += todrop;
+			m_adj(m, todrop);
+		} else {
+			m_freem(m);
+			if (mp->mp_segq)
+			    goto present;
+			else
+				return (0);
+		}
+    }
+
+	/*
+	 * Find a segment which begins after this one does.
+     * XXX: adjust for dealing with DSNs
+	 */
+	mpre = NULL;
+	for (mq = mp->mp_segq; mq != NULL; mq = mq->m_nextpkt) {
+//		printf("%s: mqdsn %lu mdsn %lu\n", __func__,
+//		    M_MPTCPDSN(mq), M_MPTCPDSN(m));
+		if (DSEQ_GT((uint64_t)M_MPTCPDSN(mq), (uint64_t)M_MPTCPDSN(m)))
+			break;
+		mpre = mq;
+//		printf("%s: mpre set, dsn %u\n", __func__, (uint32_t) M_MPTCPDSN(mq));
+	}
+
+	/*
+	 * If there is a preceding segment, it may provide some of
+	 * our data already.  If so, drop the data from the incoming
+	 * segment.  If it provides all of our data, drop us.
+     *
+     * XXX: in this case dealing with DSNs rather than TCP SEQs.
+     * So previous_dsn + previous_len compared with the DSN on
+     * the passed in mbuf.
+     *
+     * After adjustment we change the DSN on the mbuf tag. Note
+     * that the
+	 */
+	if (mpre != NULL) {
+		int i;
+
+		/* conversion to int (in i) handles seq wraparound */
+		i = M_MPTCPDSN(mpre) + mpre->m_pkthdr.len - M_MPTCPDSN(m);
+		if (i > 0) {
+			if (i >= m->m_pkthdr.len) {
+				m_freem(m);
+				/*
+				 * Try to present any queued data
+				 * at the left window edge to the user.
+				 * This is needed after the 3-WHS
+				 * completes.
+				 */
+				goto present;	/* ??? */
+			}
+			m_adj(m, i);
+			M_MPTCPDSN(m) += i;
+		}
+	}
+
+	/*
+	 * While we overlap succeeding segments trim them or,
+	 * if they are completely covered, dequeue them.
+	 */
+	while (mq) {
+		struct mbuf *nq;
+		int i;
+
+		i = (M_MPTCPDSN(m) + m->m_pkthdr.len) - M_MPTCPDSN(mq);
+		if (i <= 0)
+			break;
+		if (i < mq->m_pkthdr.len) {
+			M_MPTCPDSN(mq) += i;
+			m_adj(mq, i);
+			mp->mp_segqlen -= i;
+			break;
+		}
+
+		nq = mq->m_nextpkt;
+		mp->mp_segqlen -= mq->m_pkthdr.len;
+		m_freem(mq);
+		if (mpre)
+			mpre->m_nextpkt = nq;
+		else
+			mp->mp_segq = nq;
+		mq = nq;
+	}
+
+	/*
+	 * Insert the new (data-level) segment queue entry into place.
+	 */
+	if (mpre) {
+		m->m_nextpkt = mpre->m_nextpkt;
+		mpre->m_nextpkt = m;
+	} else {
+		mq = mp->mp_segq;
+		mp->mp_segq = m;
+		m->m_nextpkt = mq;
+	}
+	mp->mp_segqlen += m->m_pkthdr.len;
+
+present:
+
+    /* XXXNJW: check to see if first segment is in order.
+     * if so, schedule mp_input, which will append the
+     * data to the buffer. */
+	mq = mp->mp_segq;
+//    printf("%s: present, got %ju, rcv_nxt %ju\n", __func__,
+//		M_MPTCPDSN(mq), mp->ds_rcv_nxt);
+
+	SOCKBUF_LOCK(&so->so_rcv);
+	while ((mq = mp->mp_segq) != NULL &&
+		M_MPTCPDSN(mq) == mp->ds_rcv_nxt) {
+
+		mp->mp_segq = mq->m_nextpkt;
+		mp->ds_rcv_nxt += mq->m_pkthdr.len;
+		mp->mp_segqlen -= mq->m_pkthdr.len;
+
+//		printf("%s: rcv_nst now %ju\n", __func__, mp->ds_rcv_nxt);
+
+		/* XXXNJW: temp way to handle receipt of DFIN. Need to +1
+		 * ds_rcv_nxt as generally it is increased by segment length
+		 * rather than the dss_len (also currently the dss len isn't
+		 * included in the mtag) */
+		flags = M_MPTCPDSNFLAGS(mq) & MP_DFIN;
+
+		if (so->so_rcv.sb_state & SBS_CANTRCVMORE)
+			m_freem(mq);
+		else {
+			mq->m_nextpkt = NULL;
+			sbappendstream_locked(&so->so_rcv, mq, 0);
+			wakeup = 1;
+		}
+	}
+
+	if (wakeup) {
+		mp->mp_flags |= MPF_ACKNOW;
+		sorwakeup_locked(so);
+	} else
+		SOCKBUF_UNLOCK(&so->so_rcv);
+
+	return (flags);
+
+}
+
+/*
+ * Go through the process of creating a subflow - inpcb, tcpcb and a ghost
+ * socket.
+ */
+int
+mp_alloc_subflow_socket(struct socket *so, struct socket **gso)
+{
+	struct socket *sf_gso = NULL;
+	int error = 0;
+
+	/* create the ghost socket */
+	sf_gso = mp_allocghostsocket(so);
+	if (sf_gso == NULL) {
+    	error = ENOMEM;
+		goto out;
+    }
+
+	if ((sf_gso->so_options & SO_LINGER) && sf_gso->so_linger == 0)
+		sf_gso->so_linger = TCP_LINGERTIME;
+
+	*gso = sf_gso;
+
+out:
+	return (error);
+}
+
+
+/* XXXNJW: to re-think */
+static void
+mp_twstart(struct mpcb *mp)
+{
+    struct socket *so;
+    struct mppcb *mpp;
+    int acknow;
+
+    printf("%s: mp %p\n", __func__, mp);
+
+    mpp = mp->mp_mppcb;
+    MPP_LOCK_ASSERT(mpp);
+
+    so = mpp->mpp_socket;
+    soisdisconnected(so);
+
+//	if (V_nomptimewait) {
+//        mp->mp_state = MPS_M_CLOSED;
+//        mp = mp_close(mp);
+//        if (mp != NULL)
+//            MPP_UNLOCK(mp->mp_mppcb);
+//		return;
+//	}
+
+    // Maybe, so that we can call a free on the socket/mppcb
+    // without the mppcb going away?
+    // mpp_pcbref(mpp);
+
+	acknow = mp->mp_flags & MPF_ACKNOW;
+    mp->mp_state = MPS_M_TIME_WAIT;
+    mpp->mpp_flags |= MPP_TIMEWAIT;
+    mp_timer_activate(mp, MPT_TIMEOUT, mp_timeout());
+
+//    if (acknow)
+//		mp_twrespond(tw, TH_ACK);
+
+     // should we try to free the socket?
+
+}
+
+/* XXXNJW: Some flaky sequence number wrap detection
+ * Once we OR in the upper bits, if the previous DSN
+ * is larger than the data_seq_num, then data_seq_num
+ * has wrapped. Thus need to increment the upper 32 to
+ * factor in the wrap.
+ */
+static uint64_t
+mp_dsn32to64(uint64_t ds_val, uint64_t dseq)
+{
+	ds_val = (((dseq) & 0xFFFFFFFF00000000) | (uint32_t) ds_val);
+
+	/* does this dsn wrap */
+
+	/* If old dseq is greater than the new dsn, and the MSBs
+	 * of new dsn are 0, and the MSBs of old dseq are 1, then
+	 * the new dsn wraps (lower 32-bits). increment the 33rd
+	 * bit by 1. */
+	if ((uint32_t)dseq > (uint32_t)ds_val
+		&& ((uint32_t)ds_val & 0xC0000000) == 0
+		&& ((uint32_t)dseq & 0xC0000000) == 0xC0000000) {
+		mp_debug(MPSESSION, 4, 0, "%s: detected wrap on lower 32 "
+			"of ds_rcv_nxt %ju: %u, dsn map start %ju : %u\n",
+			__func__, dseq, (uint32_t)dseq,
+			ds_val, (uint32_t)ds_val);
+		ds_val += ((uint64_t) 1 << 32);
+	}
+
+	return ds_val;
+}
+
+/* XXXNJW: More flaky sequence number wrap detection,
+ * This time when converting D-ACKs to 64-bit.
+ */
+static uint64_t
+mp_ack32to64(uint64_t dack_val, uint64_t dseq)
+{
+//	printf("%s: dack_val: %u dsnd_una %u\n", (uint32_t)dack_val,
+//		(uint32_t)dseq);
+//	printf("%s: dack_val: %ju dsnd_una %ju\n", dack_val, dseq);
+	dack_val = ((dseq & 0xFFFFFFFF00000000) | (uint32_t) dack_val);
+//	printf("%s: dack_val: %ju dsnd_una %ju\n", dack_val, dseq);
+
+	/* DACK is positive and has wrapped on the lower 32
+	 * DACK is a duplicate after una has wrapped on the
+	 * lower 32. */
+	if ((uint32_t)dseq > (uint32_t)dack_val
+		&& ((uint32_t)dack_val & 0xC0000000) == 0
+		&& ((uint32_t)dseq & 0xC0000000) == 0xC0000000) {
+		mp_debug(MPSESSION, 4, 0, "%s: detected wrap on lower 32 "
+			"of ds_rcv_nxt %ju: %u, dsn map start %ju : %u\n",
+			__func__, dseq, (uint32_t)dseq,
+			dack_val, (uint32_t)dack_val);
+		dack_val += ((uint64_t) 1 << 32);
+	} else if ((uint32_t)dseq < (uint32_t)dack_val
+			&& ((uint32_t)dseq & 0xC0000000) == 0
+			&& ((uint32_t)dack_val & 0xC0000000) == 0xC0000000) {
+		dack_val -= ((uint64_t) 1 << 32);
+	}
+
+//	printf("%s: dack_val: %ju dsnd_una %ju\n", dack_val, dseq);
+	return dack_val;
+}
+
+/* If we are attempting to use an interface, need to make sure that it is up.
+ * this is more important for things like sending ADD_ADDRs on interfaces that
+ * aren't up (this can happen if the sysctl is configured to use an address,
+ * and that address happens to be down/non existent */
+int
+mp_is_if_up(struct sockaddr *l_addr) {
+	struct ifaddr *ifa = NULL;
+
+	ifa = ifa_ifwithaddr(l_addr);
+	if (!ifa)
+		return 0;
+
+	return (ifa->ifa_ifp->if_flags & IFF_UP);
+}
+
+/* If the added address is being used in the master_tp (i.e. is default route)
+ * we do not want to be advertising this (it will eventually try to bind the
+ * same address and port as the master tp, causing problems) */
+int
+mp_is_addr_default(struct sockaddr_in *l_addr, struct mpcb *mp) {
+	void *addr;
+	char inet_buf[64];
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	addr = &l_addr->sin_addr;
+	inet_ntop(AF_INET, addr, inet_buf, mp_usable_addresses[0].ss_len);
+	mp_debug(MPSESSION, 1, 0, "%s: Comparing added address %s ", __func__,
+	    inet_buf);
+
+	addr = &((struct sockaddr_in *) &mp_usable_addresses[0])->sin_addr;
+	inet_ntop(AF_INET, addr, inet_buf, mp_usable_addresses[0].ss_len);
+	mp_debug(MPSESSION, 1, 0, "with default %s\n", inet_buf);
+
+	/* is the passed-in interface the same as the default interface  */
+	if (l_addr->sin_addr.s_addr ==
+	    ((struct sockaddr_in *) &mp->mp_default_address)->sin_addr.s_addr)  {
+		mp_debug(MPSESSION, 1, 0, "%s: mp_addresses entry same as master_tp "
+		    "addr\n", __func__);
+		return 1;
+	}
+
+	mp_debug(MPSESSION, 1, 0, "%s: mp_addresses entry not same as master_tp "
+	    "addr\n", __func__);
+	return 0;
+}
+
+void
+mp_update_available_addresses(struct mpcb *mp) {
+	struct mp_add address;
+	int new_address_count = mp->mp_conn_address_count;
+	int i;
+
+	address.length = address.sub_ipver = 0;
+
+	/* Loop through available addresses. If the interface is down then it
+	 * is not included in this connection. If the interface is the same as the
+	 * default interface of the connection, it is not included. */
+	for (i = 1; i < mp->mp_conn_address_count; i++) {
+		if (!mp_is_if_up((struct sockaddr *) &mp_usable_addresses[i]) ||
+			mp_is_addr_default((struct sockaddr_in *) &mp_usable_addresses[i],
+			mp)) {
+			mp->mp_advaddr_mask &= ~(1 << i);
+			new_address_count--;
+		}
+		mp->mp_conn_address_count = new_address_count;
+	}
+
+}
+
+
+void
+mp_init(void)
+{
+	V_mpcb_zone = uma_zcreate("mpcb", sizeof(struct mpcb_mem),
+				    NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
+	uma_zone_set_max(V_mpcb_zone, maxsockets);
+
+//	V_mpcb_zone = uma_zcreate("mpsopt", sizeof(struct mp_sopt),
+//				    NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, UMA_ZONE_NOFREE);
+//	uma_zone_set_max(V_mpcb_zone, maxsockets);
+
+	MPTOK_INFO_LOCK_INIT(&mp_tokinfo_list, "mp_tok list lock"); /* XXXNJW: temp list */
+    SLIST_INIT(&mp_tokinfo_list.mpti_listhead);
+}
+
+void
+mp_destroy(void) {
+	MPTOK_INFO_LOCK_DESTROY(&mp_tokinfo_list);
+//	uma_zdestroy(V_mpsopt_zone);
+	uma_zdestroy(V_mpcb_zone);
+}
+
+SYSCTL_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_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 1d1c4c997b66 sys/netinet/mptcp_timer.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_timer.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,249 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#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/systm.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/rwlock.h>
+#include <sys/sysctl.h>
+#include <sys/sbuf.h>
+#include <sys/jail.h>
+
+#include <sys/priv.h>
+#include <sys/proc.h>
+#include <sys/protosw.h>
+
+/* XXXNJW: too many header dependencies with mptcp_var.h? (i.e. need to pull in
+ * in.h etc etc to compile. */
+#include <netinet/in.h>
+
+#include <sys/sockbuf.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+
+#include <net/vnet.h>
+#include <netinet/in_pcb.h>
+#include <netinet/tcp_var.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_timer.h>
+
+
+int	mp_backoff[MPT_MAXRXTSHIFT + 1] = {1, 1, 1, 1};
+
+
+void
+mp_timer_activate(struct mpcb *mp, int timer_type, u_int delta)
+{
+	struct callout *t_callout;
+	void *f_callout;
+	int cpu = curcpu;   /* XXNJW: look into cpuid stuff */
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	switch (timer_type) {
+		case MPT_REXMT:
+			t_callout = &mp->mp_timers->mpt_rexmt;
+			f_callout = mp_timer_rexmt;
+			break;
+		case MPT_TIMEOUT:
+			printf("%s: mp %p activate timeout\n", __func__, mp);
+			t_callout = &mp->mp_timers->mpt_timeout;
+			f_callout = mp_timer_timeout;
+			break;
+		default:
+			panic("bad timer_type");
+		}
+	if (delta == 0) {
+		callout_stop(t_callout);
+	} else {
+		callout_reset_on(t_callout, delta, f_callout, mp, cpu);
+	}
+}
+
+int
+mp_timer_active(struct mpcb *mp, int timer_type)
+{
+	struct callout *t_callout;
+
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	switch (timer_type) {
+	    case MPT_REXMT:
+		    t_callout = &mp->mp_timers->mpt_rexmt;
+		    break;
+		case MPT_TIMEOUT:
+			t_callout = &mp->mp_timers->mpt_timeout;
+			break;
+		default:
+			panic("bad timer_type");
+		}
+	return callout_active(t_callout);
+}
+
+void
+mp_timer_rexmt(void * xmp)
+{
+    struct mppcb *mpp;
+    struct mpcb *mp = xmp;
+
+    /* XXXNJW should be concerned about races with discard? */
+	if (mp == NULL) {
+		CURVNET_RESTORE();
+		return;
+	}
+
+	mpp = mp->mp_mppcb;
+    MPP_LOCK(mpp);
+
+	printf("%s: rxtshift %d, snd_nxt %u snd_una %u\n", __func__,
+	    mp->mp_rxtshift, (uint32_t) mp->ds_snd_nxt,
+	    (uint32_t) mp->ds_snd_una);
+
+	if (callout_pending(&mp->mp_timers->mpt_rexmt) ||
+		!callout_active(&mp->mp_timers->mpt_rexmt))
+        goto out;
+
+	callout_deactivate(&mp->mp_timers->mpt_rexmt);
+	if ((mpp->mpp_flags & MPP_DROPPED) != 0)
+		goto out;
+
+    /* Reached max data-level RTOs, drop the connection. */
+	if (++mp->mp_rxtshift > MPT_MAXRXTSHIFT) {
+		printf("%s: rxtshift %d reset subflows\n", __func__, mp->mp_rxtshift);
+		mp->mp_rxtshift = MPT_MAXRXTSHIFT;
+		mp_reset_all_subflows(mp);
+		(void) mp_drop(mp, ETIMEDOUT);
+		KASSERT(mp != NULL, ("%s: MP is NULL\n", __func__));
+		goto out;
+	}
+
+    /* update the timer */
+    mp->mp_rxtcur = MPTCPTV_RTOBASE * mp_backoff[mp->mp_rxtshift];
+    mp->ds_snd_nxt = mp->ds_snd_una;
+
+    printf("%s: mp_rxtcur %d ticks\n", __func__, mp->mp_rxtcur);
+
+    (void) mp_output(mp);
+
+    // mp_output now returns locked
+//    CURVNET_RESTORE();
+//    return;
+
+out:
+    MPP_UNLOCK(mpp);
+	CURVNET_RESTORE();
+}
+
+
+/* XXXNJW: timeout only set when we have no subflows (i.e.
+ * all have detached.) Thus in this case we can call mp_close
+ * */
+void
+mp_timer_timeout(void * xmp)
+{
+    struct mppcb *mpp;
+    struct mpcb *mp = xmp;
+    struct socket *so;
+
+    printf("%s: mp %p\n", __func__, mp);
+
+    /* XXXNJW should be concerned about races with discard? */
+    if (mp == NULL) {
+		CURVNET_RESTORE();
+		return;
+	}
+
+    mpp = mp->mp_mppcb;
+    MPP_LOCK(mpp);
+
+	if (callout_pending(&mp->mp_timers->mpt_timeout) ||
+		!callout_active(&mp->mp_timers->mpt_timeout))
+        goto out;
+
+	callout_deactivate(&mp->mp_timers->mpt_timeout);
+
+	mpp_pcbdrop(mpp);
+
+	/* Arrive here at end of timewait, or timed out. */
+	if (mpp->mpp_flags & MPP_TIMEWAIT) {
+		/* XXXNJW: what if we still have subflow cnt > 0 at this point? */
+		mp->mp_state = MPS_M_CLOSED;
+		so = mpp->mpp_socket;
+		if (so != NULL) {
+			/* XXXNJW: expect to have socket reference if the socket
+			 * still exists at the end of timewait. */
+			if (mpp->mpp_flags & MPP_SOCKREF) {
+				mpp->mpp_flags &= ~MPP_SOCKREF;
+				MPP_UNLOCK(mpp);
+				ACCEPT_LOCK();
+				SOCK_LOCK(so);
+				KASSERT(so->so_state & SS_PROTOREF,
+					("tcp_twclose: INP_SOCKREF && !SS_PROTOREF"));
+				so->so_state &= ~SS_PROTOREF;
+				sofree(so);
+				return;
+			} else {
+				/* should not be any cases where something else has a
+				 * reference to the socket? */
+				printf("%s: ended timewait with socket, no hard ref\n",
+				    __func__);
+			}
+		} else {
+			/* The socket has been already cleaned-up for us, free mp, mpp */
+			mp_discardcb(mp);
+			mp = NULL;
+			mpp_pcbfree(mpp);
+			mpp = NULL;
+		}
+	} else {
+		mp = mp_drop(mp, ETIMEDOUT);
+	}
+out:
+    if (mp != NULL)
+	    MPP_UNLOCK(mpp);
+	CURVNET_RESTORE();
+}
diff -r 1d1c4c997b66 sys/netinet/mptcp_timer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_timer.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,58 @@
+/*-
+ * Copyright (c) 2013-2015
+ * 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 FreeBSD Foundation and 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.
+ */
+
+#ifndef MPTCP_TIMER_H_
+#define MPTCP_TIMER_H_
+
+#define	MPTCPTV_RTOBASE	(4*hz) /* Initial RTO length (arbitrary number) */
+#define MPTCPTV_TIMEOUT (30*hz) /* Once MPTCP session enters timewait */
+#define MPTCPTV_TIMEOUTCNT 2   /* Just a default multiplier for timeout */
+
+#define	MPT_MAXRXTSHIFT	3 /* Max retransmits (arbitrary number) */
+
+struct mpcb;
+
+struct mptcp_timer {
+	struct	callout mpt_rexmt;	    /* retransmit timer */
+	struct	callout mpt_timeout;   /* timewait */
+};
+#define MPT_REXMT    0x01
+#define MPT_TIMEOUT 0x02
+
+void	mp_timer_init(void);
+void    mp_timer_activate(struct mpcb *mp, int timer_type, u_int delta);
+int     mp_timer_active(struct mpcb *mp, int timer_type);
+void	mp_timer_rexmt(void *xmp);
+void	mp_timer_timeout(void *xmp);
+
+#endif /* MPTCP_TIMER_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_types.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_types.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,42 @@
+/*-
+ * Copyright (c) 2012-2015
+ * 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 FreeBSD Foundation and 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.
+ */
+
+#ifndef MPTCP_TYPES_H_
+#define MPTCP_TYPES_H_
+
+struct multi_cb {
+	struct mpcb *mp;
+	struct inpcb *inp;
+};
+
+#endif /* MPTCP_TYPES_H_ */
diff -r 1d1c4c997b66 sys/netinet/mptcp_usrreq.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_usrreq.c	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,1216 @@
+/*-
+ * Copyright (c) 2012-2015
+ * 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 FreeBSD Foundation and 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include "opt_inet.h"
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/limits.h>
+#include <sys/endian.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mutex.h>
+#include <sys/kdb.h>
+#include <sys/kernel.h>
+#include <sys/sysctl.h>
+#include <sys/mbuf.h>
+#include <sys/queue.h>
+
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/protosw.h>
+#include <sys/proc.h>
+#include <sys/jail.h>
+
+#ifdef DDB
+#include <ddb/ddb.h>
+#endif
+
+#include <net/if.h>
+#include <net/if_var.h>
+#include <net/route.h>
+#include <net/vnet.h>
+
+#include <netinet/cc.h>
+#include <netinet/in.h>
+#include <netinet/in_pcb.h>
+#include <netinet/in_systm.h>
+#include <netinet/in_var.h>
+#include <netinet/ip_var.h>
+
+#include <netinet/tcp_fsm.h>
+#include <netinet/tcp_var.h>
+#include <netinet/tcp_timer.h>
+#include <netinet/tcp_usrreq.h>
+#include <netinet/tcpip.h>
+
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_timer.h>
+#include <netinet/mptcp_dtrace_declare.h>
+
+static int mp_attach(struct socket *so);
+static void mp_usrclosed(struct mpcb *mp);
+static void mp_disconnect(struct mpcb *mp);
+static int mp_usr_accept(struct socket *so, struct sockaddr **nam);
+static void mp_usr_detach(struct socket *so);
+static int mp_subflow_setopt(struct mpcb *mp, struct sockopt *sopt);
+static int mp_setopt(struct mpcb *mp, struct sockopt *sopt);
+static int mp_getopt(struct mpcb *mp, struct sockopt *sopt);
+
+/*
+ * When creating an MPTCP socket, now make a new mpp and mp only. A new
+ * (inp,tp,gso) subflow will be created if we issue a connect. If creating a
+ * LISTEN socket, then the call the mp_usr_listen will allocate a subflow
+ */
+static int
+mp_usr_attach(struct socket *so, int proto, struct thread *td)
+{
+    struct mppcb *mpp;
+    int error = 0;
+
+    printf("%s: so - %p\n", __func__, so);
+
+    mpp = sotomppcb(so);
+	KASSERT(mpp == NULL, ("%s: mpp != NULL", __func__));
+
+    /* This will init the mpcb */
+    error = mp_attach(so);
+    if (error)
+        goto out;
+
+    if ((so->so_options & SO_LINGER) && so->so_linger == 0)
+		so->so_linger = TCP_LINGERTIME;
+
+    /* XXXNJW: temp subflow protosw for testing */
+    sf_protosw = *so->so_proto;
+    sf_protosw.pr_usrreqs = &tcp_usrreqs;
+    sf_protosw.pr_ctloutput = tcp_ctloutput;
+
+out:
+    return error;
+}
+
+/*
+ * Initiate a connection. Will need to create and insert a new subflow, then
+ * can call tcp_usr_connect on the new subflow to actually send the SYN.
+ * tcp_output() attached an MP_CAPABLE to the outgoing SYN segment.
+ */
+static int
+mp_usr_connect(struct socket *so, struct sockaddr *nam, struct thread *td)
+{
+	struct mppcb *mpp = NULL;
+	struct mpcb *mp = NULL;
+	struct socket *sf_so = NULL;
+	struct inpcb *inp = NULL;
+	int error = 0;
+
+	printf("%s so - %p\n", __func__, so);
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mp_usr_connect: mpp == NULL"));
+	MPP_LOCK(mpp);
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		error = EINVAL;
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp_usr_connect: mp == NULL"));
+
+	if (mp->mp_state > MPS_M_CLOSED) {
+		error = EINVAL;
+		goto out;
+	}
+
+	/* creates a subflow ghost socket, inheriting state from the primary
+	 * socket (similar to sonewconn). */
+	error = mp_create_subflow_socket(so, &sf_so);
+	if (error)
+        goto out;
+
+	KASSERT(sf_so != NULL, ("%s: subflow socket NULL", __func__));
+
+	soisconnecting(so);
+
+	/* attach tcpcb and inpcb to the subflow socket */
+	error = tcp_attach(sf_so);
+	if (error)
+        goto out;
+
+	/* Insert the new sufblow pcbs and gso into sf_list */
+	error = mp_insert_subflow(mp, sf_so);
+    if (error)
+    	goto out;
+
+    /* Initiate a connection from the new subflow socket. */
+	error = (*(sf_so)->so_proto->pr_usrreqs->pru_connect)(sf_so, nam, td);
+
+	/* XXXNJW: a temporary way to store the default ports used
+	 * in this connection. */
+    inp = sotoinpcb(sf_so);
+out:
+    MPP_UNLOCK(mpp);
+	return (error);
+
+}
+
+static int
+mp_usr_bind(struct socket *so, struct sockaddr *nam, struct thread *td)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+    int error = 0;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mp_usr_bind: mpp NULL"));
+
+	MPP_LOCK(mpp);
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		error = EINVAL;
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp_usr_bind: mp NULL"));
+
+	if (mp->mp_state > MPS_M_CLOSED) {
+		error = EINVAL;
+		goto out;
+	}
+
+	INP_INFO_WLOCK(&V_tcbinfo);
+
+	/* need to call tcp_usr_bind outside of the MPP lock */
+	error = mp_bind_attach(so, mp, nam, td);
+	if (error) {
+		INP_INFO_WUNLOCK(&V_tcbinfo);
+        goto out;
+	}
+	KASSERT(mp->m_cb_ref.inp != NULL, ("mp_usr_bind: listen inp NULL"));
+
+	INP_INFO_WUNLOCK(&V_tcbinfo);
+
+out:
+	MPP_UNLOCK(mpp);
+	return (error);
+}
+
+/* The bind call should have caused a tcpcb to be
+ * allocated, so just need to check if tp exists and
+ * call tcp_usr_listen, which should set things
+ * up appropriately. */
+static int
+mp_usr_listen(struct socket *so, int backlog, struct thread *td)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+    struct inpcb *inp = NULL;
+    struct tcpcb *tp;
+	int error = 0;
+
+    mpp = sotomppcb(so);
+    KASSERT(mpp != NULL, ("mp_usr_listen: mpp == NULL"));
+    MPP_LOCK(mpp);
+
+    printf("%s: so - %p\n", __func__, so);
+
+    mp = mpptompcb(mpp);
+    KASSERT(mp != NULL, ("mp_usr_listen: mp == NULL"));
+
+    mp->mp_passive = 1;
+    inp = mp->m_cb_ref.inp;
+
+    KASSERT(inp != NULL, ("mp_usr_listen: inp == NULL"));
+    INP_WLOCK(inp);
+
+    if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+		error = EINVAL;
+		INP_WUNLOCK(inp);
+		goto out;
+	}
+	tp = intotcpcb(inp);
+
+	SOCK_LOCK(so);
+	INP_HASH_WLOCK(&V_tcbinfo);
+	if (error == 0 && inp->inp_lport == 0)
+		error = in_pcbbind(inp, (struct sockaddr *)0, td->td_ucred);
+	INP_HASH_WUNLOCK(&V_tcbinfo);
+	if (error == 0) {
+		tcp_state_change(tp, TCPS_LISTEN);
+		solisten_proto(so, backlog);
+	}
+	SOCK_UNLOCK(so);
+	INP_WUNLOCK(inp);
+
+out:
+	MPP_UNLOCK(mpp);
+	return error;
+}
+
+static int
+mp_attach(struct socket *so)
+{
+	struct mppcb *mpp;
+    struct mpcb *mp;
+	int error = 0;
+
+    /* Allocate the socket buffers. Note that for now
+     * when creating the ghost socket for subflows,
+     * they will use these sockbufs (but won't for
+     * example call sbflush). */
+    if (so->so_snd.sb_hiwat == 0 || so->so_rcv.sb_hiwat == 0) {
+		error = soreserve(so, V_tcp_sendspace, V_tcp_recvspace);
+		if (error)
+			goto out;
+	}
+
+    /* Disable for now, but should be okay to re-enable
+     * at a later point. */
+	so->so_rcv.sb_flags &= ~SB_AUTOSIZE;
+	so->so_snd.sb_flags &= ~SB_AUTOSIZE;
+
+	/* mpp is locked on return */
+	error = mpp_pcballoc(so);
+	if (error)
+		goto out;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL", __func__));
+
+    error = mp_newmpcb(mpp);
+	if (error) {
+		MPP_UNLOCK(mpp);
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("%s: mp == NULL", __func__));
+	mp->mp_state = MPS_M_CLOSED;
+
+	SDT_PROBE1(mptcp, session, mp_attach, mpcb_attached, mp);
+
+	MPP_UNLOCK(mpp);
+out:
+	return (error);
+}
+
+
+/*
+ * Mark the connection as being incapable of further output.
+ */
+static int
+mp_usr_shutdown(struct socket *so)
+{
+	struct mppcb *mpp = NULL;
+	struct mpcb *mp = NULL;
+	struct sf_handle *sfh;
+	int error = 0;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mpp == NULL"));
+    MPP_LOCK(mpp);
+
+    printf("%s: so - %p\n", __func__, so);
+
+	/* Has the primary inpcb of the connection been
+	 * previously closed? */
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		error = ECONNRESET;
+		goto out;
+	}
+
+	socantsendmore(so);
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp == NULL"));
+
+	/* MP-sessions should go through the MPTCP shutdown
+	 * states before closing the subflows */
+	if (mp->mp_connected) {
+		mp_usrclosed(mp);
+		/* Unless we were already >= M_FIN_WAIT_2, will
+		 * need to send a DFIN to the foreign host. mp_output
+		 * unlocks mp, mpp */
+		if (!(mpp->mpp_flags & MPP_DROPPED))
+			error = mp_output(mp);
+	} else if (mp->subflow_cnt > 0) {
+		KASSERT(mp->subflow_cnt == 1, ("%s: multiple subflows on non-mptcp "
+			"connection\n", __func__));
+		/* XXXNJW: Just using the first subflow for now */
+		sfh = TAILQ_FIRST(&mp->sf_list);
+		KASSERT(sfh->sf_so != NULL, ("%s: sf_so == NULL", __func__));
+		printf("%s: shutdown subflow socket %p\n", __func__, sfh->sf_so);
+		(*(sfh->sf_so)->so_proto->pr_usrreqs->pru_shutdown)(sfh->sf_so);
+	}
+
+out:
+	MPP_UNLOCK(mpp);
+	return (error);
+}
+
+static int
+mp_usr_disconnect(struct socket *so)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp = NULL;
+	struct sf_handle *sfh;
+	int error = 0;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mp_usr_disconnect: mpp == NULL"));
+
+	MPP_LOCK(mpp);
+
+	printf("%s: so - %p\n", __func__, so);
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		error = ECONNRESET;
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp_usr_disconnect: mp == NULL"));
+
+	/* (1) A mp_connected session should go through the
+	 * multipath shutdown states, which involves sending
+	 * a D-FIN and so forth. An MP connection that was
+	 * never connected, and never had and subflows will
+	 * drop straight through here (as there is nothing to
+	 * disconnect). mp_usr_close will cause mp_close to
+	 * be called later.
+	 * (2) If a MPTCP connection was never established, but
+	 * we have subflows, mark the socket as disconnecting
+	 * and call pru_disconnect on the subflows now. */
+	if (mp->mp_connected) {
+		mp_disconnect(mp);
+	} else {
+		soisdisconnecting(so);
+		sbflush(&so->so_rcv);
+
+		if (mp->subflow_cnt == 0) {
+			/* Will drop the mppcb, but not free any control blocks */
+			mp = mp_close(mp);
+			KASSERT(mp != NULL, ("mp_disconnect: mp_close() returned NULL"));
+		} else {
+			/* XXXNJW: Just using the first subflow for now */
+			sfh = TAILQ_FIRST(&mp->sf_list);
+			if (!(sfh->sf_flags & (SFHS_MPENDED|SFHS_DISCONNECTING))) {
+				KASSERT(sfh->sf_so != NULL, ("%s: sf_so == NULL", __func__));
+				printf("%s: disconnect subflow socket %p\n", __func__, sfh->sf_so);
+				sfh->sf_flags |= SFHS_DISCONNECTING;
+				error = (*(sfh->sf_so)->so_proto->pr_usrreqs->pru_disconnect)
+					(sfh->sf_so);
+			}
+		}
+	}
+
+out:
+    MPP_UNLOCK(mpp);
+	return (error);
+}
+
+/* XXXNJW: Try to keep mp_disconnect relevant only
+ * at MP level. I.e. for connections that reached
+ * mp_connected. Other cases should not come through
+ * here (e.g. still connecting, infinite mapped) . */
+static void
+mp_disconnect(struct mpcb *mp)
+{
+	printf("%s\n", __func__);
+
+	struct mppcb *mpp = mp->mp_mppcb;
+	struct socket *so = mpp->mpp_socket;
+
+	MPP_LOCK_ASSERT(mpp);
+
+	if (mp->subflow_cnt == 0) {
+		/* If there are no subflows, should close everything
+		 * now mp won't return as NULL, as socket won't be
+		 * freed  */
+		printf("%s: subflow cnt == 0\n", __func__);
+		mp = mp_close(mp);
+		KASSERT(mp != NULL, ("mp_disconnect: mp_close() returned NULL"));
+	} else {
+		printf("%s: set mp->socket disconnecting. sf_cnt %d\n", __func__,
+		    mp->subflow_cnt);
+		/* The session has subflows and reached a connected state */
+		soisdisconnecting(so);
+		sbflush(&so->so_rcv);
+		/* We have an active MPTCP session, therefore need to
+		 * go through MPTCP shutdown states */
+		mp_usrclosed(mp);
+		if (!(mpp->mpp_flags & MPP_DROPPED))
+		    mp_output(mp);
+	}
+}
+
+static void
+mp_usr_close(struct socket *so)
+{
+	struct mppcb *mpp = NULL;
+	struct mpcb *mp = NULL;
+    struct sf_handle *sfh = NULL;
+    struct socket *sf_so;
+    struct inpcb *sf_inp;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL\n", __func__));
+	MPP_LOCK(mpp);
+
+	printf("%s: so - %p\n", __func__, so);
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("%s: mp == NULL\n", __func__));
+
+	/* If an MPTCP connection was established at some point, */
+	if (mp->mp_connected) {
+		if (!(mpp->mpp_flags & MPP_TIMEWAIT) &&
+			!(mpp->mpp_flags & MPP_DROPPED))
+			mp_disconnect(mp);
+
+		if (!(mpp->mpp_flags & MPP_TIMEWAIT) &&
+			!(mpp->mpp_flags & MPP_DROPPED)) {
+			SOCK_LOCK(so);
+			so->so_state |= SS_PROTOREF;
+			SOCK_UNLOCK(so);
+			mpp->mpp_flags |= MPP_SOCKREF;
+		}
+	} else if (mp->subflow_cnt == 1) {
+		/* Might need to wait on tcp to close before freeing,
+		 * so take strong reference (prevents sofree from
+		 * detaching the mppcb). Must always wait on subflows to
+		 * free themselves before we can free the MP-layer
+		 * control blocks (as subflows still dereference the
+		 * mpcb) */
+		if (!(mpp->mpp_flags & MPP_TIMEWAIT) &&
+			!(mpp->mpp_flags & MPP_DROPPED)) {
+			SOCK_LOCK(so);
+			so->so_state |= SS_PROTOREF;
+			SOCK_UNLOCK(so);
+			mpp->mpp_flags |= MPP_SOCKREF;
+		}
+
+		/* Initiate a close on the subflow socket. The subflow
+		 * will notify the mp-layer when it discards the tcpcb.
+		 * Call sorele after. If the subflow is already DROPPED,
+		 * then subflow will be detached and freed. Otherwise
+		 * will be freed at the end of time-wait or on entering
+		 * tcp_close.
+		 *
+		 * XXXNJW: temp, just grab the first sublfow as this
+		 * should be the only one we have. */
+		sfh = TAILQ_FIRST(&mp->sf_list);
+		if (sfh->sf_so != NULL) {
+			sf_so = sfh->sf_so;
+			KASSERT(sf_so != NULL, ("%s: sf so == NULL", __func__));
+
+			sf_inp = sotoinpcb(sf_so);
+			KASSERT(sf_inp != NULL, ("%s: inp == NULL", __func__));
+
+			(*sf_so->so_proto->pr_usrreqs->pru_close)(sf_so);
+
+			/* If SS_PROTOREF is not held, should be okay to free
+			 * the subflow socket here. Otherwise the subflow socket
+			 * is freed once the subflow has finished using it (e.g.
+			 * it might still be in the process of disconnecting at
+			 * this point). */
+			if (!(sf_so->so_state & SS_PROTOREF)) {
+				printf("%s: free and release subflow socket\n", __func__);
+
+				/* XXXNJW: temp. To make sure that the mp-layer
+				 * doesn't try to access this subflow after this
+				 * point. */
+				sfh->sf_flags |= SFHS_MPENDED;
+				sfh->sf_so = NULL;
+
+				/* sofree on the subflow socket */
+				mp_subflow_release_socket(sf_so);
+
+				/* Decrement subflow count. this will result in a
+				 * call to mp_close (as we only have a single sublow
+				 * in non mp_connected sessions. */
+				KASSERT(mp != NULL, ("%s: mp NULL\n", __func__));
+				if (mp_detach_subflow_locked(mp))
+					return;
+			}
+		}
+
+	} else if (!(mp->mp_connected) && (mp->subflow_cnt == 0)) {
+		printf("%s: subflow already detached\n", __func__);
+		if ((mp = mp_close(mp)) == NULL)
+		    return;
+	}
+
+	MPP_UNLOCK(mpp);
+}
+
+static void
+mp_usrclosed(struct mpcb *mp)
+{
+	MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	switch (mp->mp_state) {
+	    /* The closed case will be handled earlier than here. */
+		case MPS_M_CLOSED:
+			printf("%s: already in state CLOSED\n", __func__);
+			if(mp->subflow_cnt == 0)
+			    mp = mp_close(mp);
+			KASSERT(mp != NULL,
+			    ("mp_usrclosed: mp_close() returned NULL"));
+			break;
+		case MPS_M_ESTABLISHED:
+			mp->mp_state = MPS_M_FIN_WAIT_1;
+			break;
+		case MPS_M_CLOSE_WAIT:
+			mp->mp_state = MPS_M_LAST_ACK;
+			break;
+	}
+
+	if (mp->mp_state >= MPS_M_FIN_WAIT_2) {
+		printf("%s: >= FW2\n", __func__);
+		soisdisconnected(mp->mp_mppcb->mpp_socket);
+
+		/* Prevent the connection hanging in FIN_WAIT_2
+		 * indefinitely. Since we call 'close_all_subflows'
+		 * when we move to M_FW2, it is possible for the
+		 * subflows to all go through shutdown without
+		 * sending a DFIN back to US. */
+		if (mp->mp_state == MPS_M_FIN_WAIT_2) {
+		    // start a timeout. base this on a longer 'idle'
+			// period (in case the other side still plans on
+			// sending)? just using the hard-coded timer for now
+			if (!mp_timer_active(mp, MPT_TIMEOUT))
+				mp_timer_activate(mp, MPT_TIMEOUT, MPTCPTV_TIMEOUT);
+		}
+	}
+
+}
+
+static void
+mp_usr_abort(struct socket *so)
+{
+	struct mppcb *mpp = NULL;
+	struct mpcb *mp = NULL;
+
+	printf("%s: not implemented, stopping\n", __func__);
+	kdb_break();
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mp_usr_abort: mpp == NULL"));
+
+	MPP_LOCK(mpp);
+	KASSERT(mpp->mpp_socket != NULL,
+	    ("mp_usr_abort: mp_socket == NULL"));
+
+	if (!(mpp->mpp_flags & MPP_TIMEWAIT) &&
+	    !(mpp->mpp_flags & MPP_DROPPED)) {
+		mp = mpptompcb(mpp);
+		KASSERT(mp != NULL, ("mp_usr_abort: mp == NULL"));
+//		mp_drop(mp, ECONNABORTED);
+	}
+	if (!(mpp->mpp_flags & MPP_DROPPED)) {
+		SOCK_LOCK(so);
+		so->so_state |= SS_PROTOREF;
+		SOCK_UNLOCK(so);
+		mpp->mpp_flags |= MPP_SOCKREF;
+	}
+
+	MPP_UNLOCK(mpp);
+}
+
+/* XXXNJW temporary - just pulls the first subflow
+ * and sends */
+static int
+mp_usr_send(struct socket *so, int flags, struct mbuf *m,
+    struct sockaddr *nam, struct mbuf *control, struct thread *td)
+{
+	int mp_outflags = 0, error = 0;
+	struct mppcb *mpp = NULL;
+	struct mpcb *mp = NULL;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL\n", __func__));
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		if (control)
+			m_freem(control);
+		if (m)
+			m_freem(m);
+		error = ECONNRESET;
+		goto out;
+	}
+
+	/* TCP doesn't do control messages (rights, creds, etc) */
+	if (control) {
+		if (control->m_len) {
+			m_freem(control);
+			if (m)
+				m_freem(m);
+			error = EINVAL;
+			goto out;
+		}
+		m_freem(control);	/* empty control, just free it */
+	}
+
+	/* no OOB for the moment */
+	if (flags & PRUS_OOB) {
+		if (m)
+			m_freem(m);
+		error = EINVAL;
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp_usr_send: mp == NULL"));
+
+	if (nam && !mp->mp_connected) {
+		panic("mp_usr_send: transport not yet connected\n");
+	}
+
+	/* no flags for now. */
+	sbappendstream(&so->so_snd, m, 0);
+	if (flags & PRUS_EOF) {
+		printf("%s: got PRUS_EOF\n", __func__);
+		/*
+		 * Close the send side of the connection after
+		 * the data is sent.
+		 */
+		socantsendmore(so);
+		mp_usrclosed(mp);
+	}
+	if (!(mpp->mpp_flags & MPP_DROPPED)) {
+		if (flags & PRUS_MORETOCOME)
+			mp_outflags |= PRUS_MORETOCOME;
+
+		/* mp_output _no longer_ unlocks */
+		if (mp->mp_connected)
+		    error = mp_output(mp);
+		else
+			error = mp_standard_output(mp);
+	}
+
+out:
+	MPP_UNLOCK(mpp);
+	return error;
+}
+
+/* XXXNJW temporary Should really just be tested
+ * for a valid connection then calling mp_output() */
+static int
+mp_usr_rcvd(struct socket *so, int flags)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	int error = 0;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL\n", __func__));
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		error = ECONNRESET;
+		goto out;
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("%s: mp == NULL\n", __func__));
+
+	if (mp->mp_connected)
+        error = mp_output(mp);
+	else
+		error = mp_standard_output(mp);
+
+out:
+	MPP_UNLOCK(mpp);
+	return error;
+}
+
+static int
+mp_usr_accept(struct socket *so, struct sockaddr **nam)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	struct sf_handle *sf;
+	struct inpcb *inp;
+	struct tcpcb *tp;
+	struct in_addr addr;
+	int error = 0;
+	in_port_t port = 0;
+
+	if (so->so_state & SS_ISDISCONNECTED)
+		return (ECONNABORTED);
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL", __func__));
+	MPP_LOCK(mpp);
+
+	INP_INFO_RLOCK(&V_tcbinfo);
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("%s: mp == NULL", __func__));
+
+	/* XXXNJW: Temp while testing */
+	sf = TAILQ_FIRST(&mp->sf_list);
+	inp = sotoinpcb(sf->sf_so);
+	KASSERT(inp != NULL, ("%s: inp == NULL", __func__));
+
+	INP_WLOCK(inp);
+	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+		error = ECONNABORTED;
+		goto out;
+	}
+	tp = intotcpcb(inp);
+
+	/*
+	 * We inline in_getpeeraddr and COMMON_END here,
+	 * so that we can copy the data of interest and
+	 * defer the malloc until after we release the lock.
+	 */
+	port = inp->inp_fport;
+	addr = inp->inp_faddr;
+
+out:
+	INP_WUNLOCK(inp);
+	INP_INFO_RUNLOCK(&V_tcbinfo);
+	MPP_UNLOCK(mpp);
+
+	if (error == 0)
+		*nam = in_sockaddr(port, &addr);
+	return error;
+}
+
+/* XXXNJW temp for testing. Will free any proto blocks
+ * associated with the MPTCP socket. There is no MP
+ * session time-wait; so this occurs after the tcp
+ * subflow time-wait period (if active closer).
+ * NB: The MP_LOCK isn't acquired here either. */
+static void
+mp_usr_detach(struct socket *so)
+{
+	printf("%s: so %p\n", __func__, so);
+
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	struct inpcb *inp = NULL;
+	struct tcpcb *tp = NULL;
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("mp_usr_detach: mpp NULL"));
+    MPP_LOCK(mpp);
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("mp_usr_detach: mp NULL"));
+
+	KASSERT(mpp->mpp_socket != NULL,
+	    ("mp_usr_detach: mpp_socket == NULL"));
+	KASSERT(so->so_pcb == mpp, ("mp_detach: so_pcb != mpp"));
+	KASSERT(mpp->mpp_socket == so, ("mp_detach: mpp_socket != so"));
+
+	/* If there is a listen tp, should discard it */
+	if ((inp = mp->m_cb_ref.inp)) {
+		INP_INFO_WLOCK(&V_tcbinfo);
+		INP_WLOCK(inp);
+		tp = intotcpcb(inp);
+		KASSERT(tp != NULL, ("%s: tp NULL\n", __func__));
+		tcp_discardcb(tp);
+		inp->inp_socket = NULL;
+		in_pcbfree(inp);
+		INP_INFO_WUNLOCK(&V_tcbinfo);
+	}
+
+	if (mp->mp_connected) {
+		if (mpp->mpp_flags & MPP_TIMEWAIT) {
+			/* 1 - M_TIMEWAIT state has been discarded
+			 * 2 - Still in TIMEWAIT */
+			if (mpp->mpp_flags & MPP_DROPPED) {
+				mp_discardcb(mp);
+				mpp_pcbdetach(mpp);
+				mpp_pcbfree(mpp);
+			} else {
+				mpp_pcbdetach(mpp);
+				MPP_UNLOCK(mpp);
+			}
+		} else {
+			if ((mpp->mpp_flags & MPP_DROPPED) ||
+				(mp->mp_state == MPS_M_CLOSED)) {
+				mp_discardcb(mp);
+				mpp_pcbdetach(mpp);
+				mpp_pcbfree(mpp);
+			} else {
+				printf("%s: detached an mpp that is not "
+					"MPP_DROPPED or MPS_M_CLOSED\n", __func__);
+				mpp_pcbdetach(mpp);
+				MPP_UNLOCK(mpp);
+			}
+		}
+	} else {
+		if ((mpp->mpp_flags & MPP_DROPPED) ||
+			(mp->mp_state == MPS_M_CLOSED)) {
+			mp_discardcb(mp);
+			mpp_pcbdetach(mpp);
+			mpp_pcbfree(mpp);
+		} else {
+			/* This case shouldn't happen */
+			printf("%s: detached an mp session not MPP_DROPPED or "
+				  "MPS_M_CLOSED\n", __func__);
+			mpp_pcbdetach(mpp);
+			MPP_UNLOCK(mpp);
+		}
+	}
+
+
+}
+
+static int
+mp_usr_rcvoob(struct socket *so, struct mbuf *m, int flags)
+{
+	int error = 0;
+//	struct inpcb *inp;
+//	struct mpcb *mp = NULL;
+//	struct sf_handle *sf = NULL;
+//
+//
+//	inp = sotoinpcb(so);
+//	KASSERT(inp != NULL, ("%s: inp == NULL\n", __func__));
+//	INP_WLOCK(inp);
+//	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
+//		error = ECONNRESET;
+//		INP_WUNLOCK(inp);
+//		goto out;
+//	}
+//
+//	mp = intompcb(inp);
+//	KASSERT(mp != NULL, ("%s: mp == NULL\n", __func__));
+//	mp_pcbref(mp);
+//	INP_WUNLOCK(inp);
+//
+//	sf = TAILQ_FIRST(&mp->sf_list);
+//	KASSERT(sf != NULL, ("%s: sf NULL\n", __func__));
+//	error = tcp_usr_rcvoob(sf->sf_so, m, flags);
+//
+//out:
+	return error;
+}
+
+struct protosw sf_protosw;
+
+#ifdef INET
+struct pr_usrreqs mptcp_usrreqs = {
+	.pru_abort =		mp_usr_abort,
+	.pru_accept =		mp_usr_accept,
+	.pru_attach =		mp_usr_attach,
+	.pru_bind =	        mp_usr_bind,
+	.pru_connect =		mp_usr_connect,
+	.pru_control =		in_control,
+	.pru_detach =		mp_usr_detach,
+	.pru_disconnect =	mp_usr_disconnect,
+	.pru_listen =		mp_usr_listen,
+	.pru_peeraddr =		mpp_getpeeraddr,
+	.pru_rcvd =         mp_usr_rcvd,
+	.pru_rcvoob =		mp_usr_rcvoob,
+	.pru_send =	        mp_usr_send,
+	.pru_shutdown =		mp_usr_shutdown,
+	.pru_sockaddr =		mpp_getsockaddr,
+	//.pru_sosetlabel =	in_pcbsosetlabel,
+	.pru_close =		mp_usr_close,
+	.pru_sosend = sosend_generic,
+	.pru_soreceive = soreceive_generic,
+};
+#endif /* INET */
+
+/* XXNJW: Currently just go through all the subflows
+ * and set the same sock opt. These settings also need
+ * to propagate to new subflows being added later.
+ *
+ * sosetopt will set options on the mppcb socket before
+ * we get here. IPPROTO_IP options, or socket options
+ * that might modify an inpcb (e.g. SO_SETFIB) will not
+ * be set on the mppcb. So basically at this point
+ * socket-level stuff has been already set, and we just
+ * filter and propagate options to the subflows (at levels
+ * socket, inpcb, tcpcb).
+ */
+int
+mp_ctloutput(struct socket *so, struct sockopt *sopt)
+{
+	struct mppcb *mpp;
+	struct mpcb *mp;
+	int error = 0;
+
+	if (sopt->sopt_level != SOL_SOCKET && sopt->sopt_level != IPPROTO_TCP
+		    && sopt->sopt_level != IPPROTO_IP)
+		return(EINVAL);
+
+	mpp = sotomppcb(so);
+	KASSERT(mpp != NULL, ("%s: mpp == NULL", __func__));
+	MPP_LOCK(mpp);
+
+	if (mpp->mpp_flags & (MPP_TIMEWAIT | MPP_DROPPED)) {
+		MPP_UNLOCK(mpp);
+		return (ECONNRESET);
+	}
+
+	mp = mpptompcb(mpp);
+	KASSERT(mp != NULL, ("%s: mp == NULL", __func__));
+
+	switch (sopt->sopt_dir) {
+		case SOPT_SET:
+			error = mp_setopt(mp, sopt);
+			break;
+		case SOPT_GET:
+			error = mp_getopt(mp, sopt);
+			break;
+		default:
+			break;
+	}
+
+	MPP_UNLOCK(mpp);
+
+	return error;
+}
+
+/* Sits between mp_ctloutput and sosetopt of the
+ * subflows. Here we filter the options to a subset
+ * that has relevance at the subflow level.
+ *
+ * XXXNJW: currently a very small subset of Socket/TCP/IP
+ * options are supported. This can/should expand in
+ * the future. */
+static int
+mp_setopt(struct mpcb *mp, struct sockopt *sopt)
+{
+    int error = 0, optval;
+    struct mp_sopt *m_sopt = NULL;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+	SDT_PROBE3(mptcp, session, mp_setopt, entry, mp,
+	    sopt->sopt_level, sopt->sopt_name);
+
+    /* (1) Might need to apply options to subflow
+     * sockets. Only some options should be applied.
+     * XXXNJW: Should also store what options have been
+     * set, so that new subflows can inherit these on
+     * creation.
+     * (2) IP options.
+     * (3) TCP options. */
+    if (sopt->sopt_level == SOL_SOCKET) {
+    	switch(sopt->sopt_name) {
+		/* XXXNJW: For now just pass on this subset of
+		 * options to subflow socket */
+    	case SO_DEBUG:
+    	case SO_KEEPALIVE:
+    	case SO_LINGER:
+    	/* The following make changes in the inpcb (via
+    	 * ip_ctloutput) */
+    	case SO_REUSEADDR:
+		case SO_REUSEPORT:
+		case SO_SETFIB:
+    	    break;
+
+		default:
+			error = ENOPROTOOPT;
+			goto out;
+		}
+    } else if (sopt->sopt_level == IPPROTO_IP) {
+    	switch(sopt->sopt_name) {
+		/* XXXNJW: just a minimal set of IP options allowed
+		 * through for the moment */
+		case IP_OPTIONS:
+		case IP_TOS:
+		case IP_TTL:
+		case IP_MINTTL:
+			break;
+    	default:
+    		error = ENOPROTOOPT;
+    		goto out;
+    	}
+    } else { /* IPPROTO_TCP */
+    	switch(sopt->sopt_name) {
+		/* XXXNJW: For now pass on this subset of options
+		 * to the subflow tcpcb */
+		case TCP_NODELAY:
+		case TCP_NOPUSH:
+		case TCP_MAXSEG:
+		case TCP_CONGESTION:
+		case TCP_KEEPIDLE:
+		case TCP_KEEPINTVL:
+		case TCP_KEEPINIT:
+		case TCP_KEEPCNT:
+			break;
+		default:
+			error = ENOPROTOOPT;
+			goto out;
+		}
+    }
+
+    if ((error = sooptcopyin(sopt, &optval, sizeof (optval),
+        sizeof (optval))) != 0)
+    	goto out;
+
+    /* If this is the first time setting this option, create
+     * and insert (into mp session list) a new mp_sopt */
+    if ((m_sopt = mp_locate_mp_sopt(mp, sopt)) == NULL)
+    	m_sopt = mp_alloc_mp_sopt();
+
+    if (m_sopt == NULL) {
+		error = ENOBUFS;
+		goto out;
+	} else {
+		/* Populate/refresh option so that it can be applied
+		 * to new subflows when they are created. */
+    	m_sopt->sopt_level = sopt->sopt_level;
+    	m_sopt->sopt_name = sopt->sopt_name;
+    	m_sopt->sopt_val = optval;
+    }
+
+    /* Apply the option on all the currently existing subflows */
+   	error = mp_subflow_setopt(mp, sopt);
+
+out:
+	return error;
+}
+
+/* XXXNJW: For now just pulling the options that have
+ * been set on the first subflow (assuming a subflow
+ * has been created). TODO: keep track of what options
+ * have been set (or maybe some defaults) and return
+ * these. */
+static int
+mp_getopt(struct mpcb *mp, struct sockopt *sopt)
+{
+    int error = 0;
+    struct sf_handle *sf;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    SDT_PROBE3(mptcp, session, mp_getopt, entry, mp,
+    	sopt->sopt_level, sopt->sopt_name);
+
+	/* We have either IP or TCP level options here (as
+	 * socket level are processed in sogetopt).
+	 *
+	 * Need to filter according to the options that we
+	 * have, and then return the values that are stored
+	 * in mp->mpsopt_list. */
+
+    /* Only return a subset TCP or IP options, as we don't
+     * allow setting of all the typical TCP socket opts (for now)*/
+
+    switch (sopt->sopt_level) {
+    case IPPROTO_IP:
+        switch (sopt->sopt_name) {
+		case IP_OPTIONS:
+		case IP_TOS:
+		case IP_TTL:
+		case IP_MINTTL:
+			break;
+		default:
+			error = ENOPROTOOPT;
+			goto out;
+		}
+        break;
+	case IPPROTO_TCP:
+		switch (sopt->sopt_name) {
+		case TCP_NODELAY:
+		case TCP_MAXSEG:
+		case TCP_NOPUSH:
+		case TCP_CONGESTION:
+			break;
+		default:
+			error = ENOPROTOOPT;
+			goto out;
+		}
+		break;
+	default:
+		error = ENOPROTOOPT;
+		goto out;
+    }
+    /* XXXNJW: should base this off of what we have in
+     * mpsopt_list, but since all subflows have the same
+     * options for now, just  */
+    if (mp->subflow_cnt) {
+		sf = TAILQ_FIRST(&mp->sf_list);
+		if (sf != NULL) {
+			error = sogetopt(sf->sf_so, sopt);
+		}
+	} else
+		sopt->sopt_valsize = 0;
+
+out:
+	return error;
+}
+
+
+/* Itereate through the subflows and call sosetopt. May
+ * need to filter options as it might be incorrect to
+ * set options on some subflows, depending on their
+ * current state. */
+static int
+mp_subflow_setopt(struct mpcb *mp, struct sockopt *sopt)
+{
+    int error = 0;
+
+    MPP_LOCK_ASSERT(mp->mp_mppcb);
+
+    /* XXXNJW: Once the options have been filtered,
+     * iterate through and set on all subflows.
+     *
+     * Options must be protocol-specific (i.e. TCP)
+     * and also subflow-relevant */
+
+    /* XXNJW: From here on should be applying option
+     * to subflows */
+    if (mp->subflow_cnt) {
+		struct sf_handle *sf;
+		/* Iterate here */
+		sf = TAILQ_FIRST(&mp->sf_list);
+		if (sf != NULL) {
+			printf("%s: set opt level %d name %d on sf %p\n", __func__,
+				sopt->sopt_level, sopt->sopt_name, sototcpcb(sf->sf_so));
+			error = sosetopt(sf->sf_so, sopt);
+		}
+
+		/* Also store a copy of the currently applied
+		 * socket options, so newly created subflows can
+		 * apply these also. */
+    }
+    return error;
+}
+
diff -r 1d1c4c997b66 sys/netinet/mptcp_var.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/mptcp_var.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,715 @@
+/*-
+ * Copyright (c) 2012-2015
+ * 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 FreeBSD
+ * Foundation and 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.
+ */
+
+#ifndef MPTCP_VAR_H_
+#define MPTCP_VAR_H_
+
+#include <netinet/in.h>
+#include <netinet/mptcp_types.h>
+
+#define MAX_SUBFLOWS 8
+#define MAX_ADDRS 	8
+#define MAX_SUBFLOW_REXMIT    3
+
+#define	BTOHEX_MSBLTOR	0x01
+#define	BTOHEX_MSBRTOL	0x02
+
+
+/* Is mptcp enabled? Determines whether MP_CAPABLE is put into outgoing SYNs,
+ * or responded to if we receive a SYN + MP_CAPABLE */
+//VNET_DECLARE(int, tcp_do_mptcp);
+//#define	V_tcp_do_mptcp	VNET(tcp_do_mptcp)
+
+/* for malloc'ing list heads */
+MALLOC_DECLARE(M_REASSLISTHEAD);
+
+/*
+ * 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];
+
+struct syncache;
+struct ip;
+
+/* User-protocol handle */
+extern struct pr_usrreqs mptcp_usrreqs;
+
+/* XXXNJW: A temp struct used to map mpcbs to mp session tokens. */
+extern struct mp_sessions mp_tokinfo_list;
+
+/* XXXNJW: A temp protosw that is used when creating subflows (allows them to
+ * use standard TCP pr_usrreqs) */
+extern struct protosw sf_protosw;
+
+
+/* control output (set socket options) */
+int mp_ctloutput(struct socket *, struct sockopt *);
+
+#define sotomppcb(so) ((struct mppcb *)(so)->so_pcb)
+
+/* Macros to initialize mptcp sequence numbers for
+ * send and receive from initial tcp send and receive
+ * sequence numbers. Used by the first subflow only,
+ * as we operate in infinite mapped mode until mptcp
+ * session is fully established.
+ */
+#define	mp_rcvseqinit(mp,tp) \
+	(mp)->ds_rcv_nxt = (tp)->irs + 1
+
+#define	mp_sendseqinit(mp,tp) \
+	(mp)->ds_snd_una = (mp)->ds_snd_nxt = (mp)->ds_map_max = \
+	    (mp)->ds_map_min = (tp)->iss + 1
+
+#define M_MAXSFREXMIT() \
+	V_mpsubflowrexmits ? V_mpsubflowrexmits : MAX_SUBFLOW_REXMIT
+
+
+/* MPTCP Subflow handle.
+ *
+ * Accessed under the SF_LIST_RLOCK and SF_LIST_WLOCK and protected by the
+ * INP_WLOCK (from the subflow INET PCB).
+ * XXXNJW: Is this a sensible way to do it?
+ */
+struct sf_handle {
+	TAILQ_ENTRY(sf_handle) next_sf_handle;
+	struct socket *sf_so; /* subflow socket */
+    struct protosw *sf_protosw;	 /* use standard TCP protosw for subflows */
+    int    sf_flags;    /* subflow state flags */
+};
+
+struct mp_sched {
+	struct sf_handle *last_sf_selected;
+};
+
+/* Struct to hold socket options for the connection */
+struct mp_sopt {
+       TAILQ_ENTRY(mp_sopt) next_mp_sopt;
+       int sopt_level; /* option level (IPPROTO_TCP/SOL_SOCKET) */
+       int sopt_name; /* option name */
+       int sopt_val;  /* option value */
+};
+
+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);
+
+/* XXXNJW: temp. list of mpcb-token structs */
+SLIST_HEAD(mpti_listhead, mpcb_tokinfo);
+
+/*
+ * Multi-path Control Block.
+ *
+ * Protected by MP_LOCK (m) and the MP_INNER_LOCK (i).
+ *
+ * Fields protected by the inner lock can be accessed outside of (m). The (m)
+ * lock should not be taken while holding the (i) lock, but the (i) lock can
+ * be taken while holding the (m) lock.
+ *
+ * XXXNJW: There is a bunch of path management stuff in here that needs to be
+ * moved elsewhere.
+ */
+struct mpcb {
+	struct mppcb *mp_mppcb;     /* (m) back pointer to multipath pcb */
+	struct multi_cb m_cb_ref;   /* (m) pointer to self and a "listen" INP */
+	uint8_t	mp_connected:1,     /* (m) did session ever connect? */
+			mp_session:1,       /* (i) is this a mptcp session? */
+			mp_passive:1,       /* (i) passive opener */
+			csum_enabled:1;	    /* (i) using csum with data segments */
+	int	mp_state;               /* state of mptcp connection */
+	u_int mp_flags;             /* (i) mpcb session-related flags*/
+	u_int16_t mp_event_flags;   /* E.g. mp-signaling events */
+
+	struct taskqueue *mp_tq;	/* per-mp private task queue */
+	struct sched_algo *sched_algo; /* scheduling algorithm to use */
+
+    /* Subflows */
+	TAILQ_HEAD(sf_handle_head, sf_handle) sf_list; /* (m) List of sf handles */
+	int mp_conn_address_count; /* number of addresses available to session */
+	int subflow_cnt;			/* (i) total number of subflows */
+
+	/* TCP socket options */
+	TAILQ_HEAD(mp_sopt_head, mp_sopt) mp_sopt_list; /* (m) List of socket options */
+
+
+	struct mbuf *mp_input_segq; /* List of segments to process in mp_input */
+	struct mbuf *mp_segq;       /* reass list of data-level segments */
+	int mp_segqlen;             /* bytes in m_segq */
+
+	u_int	t_rcvtime;		    /* inactivity time */
+	u_int	t_starttime;		/* time connection was established */
+
+	/* Data-level retransmits */
+	struct mptcp_timer *mp_timers;	/* All the MPTCP timers in one struct */
+	int mp_rxtcur;				   /* Current data-level RTO in ticks */
+    int mp_rxtshift;               /* log(2) of rexmt exp. backoff */
+
+	/* tokens, seq nums, windows */
+	uint32_t local_token; 	/* local token for mptcp connection */
+	uint32_t remote_token;  /* (i) token generated by remote host */
+	uint64_t local_key; 	/* (i) 64-bit key sent by this host */
+	uint64_t remote_key; 	/* (i) 64-bit key from remote host */
+
+	uint64_t ds_idsn;		/* (i) data seq initial data seq number */
+	uint64_t ds_snd_una;	/* (i) oldest unacknowledged seq, data space */
+	uint64_t ds_snd_max;	/* (i) max send seq num, data space */
+    uint64_t ds_snd_nxt;    /* (i) next sqn to send */
+
+	uint64_t ds_map_max;	/* (i) Seq num of next byte to be mapped to a SF */
+	uint64_t ds_map_min;	/* (i) Lowest seq no. currently in a map (may be below UNA) */
+	uint32_t ds_snd_wnd;	/* (i) current send window */
+
+	uint64_t ds_idsr;		/* (i) Initial data sequence receive */
+	uint64_t ds_rcv_nxt;	/* (i) next expected data seq number */
+	uint64_t ds_last_rcvd;	/* the last received ds seq number */
+	uint32_t ds_rcv_wnd;	/* (i*) Receive window to be advertised by sub-flows */
+	uint64_t ds_last_dack_sent; /* Last sent data-level ACK */
+
+	/* CC stuff. To go elsewhere */
+	uint64_t alpha;			/* alpha value for congestion control */
+	uint64_t total_cwnd;	/* total cwnd of all subflows */
+
+	/*
+	 * The "path manager" of sorts, just an array of addresses
+	 * that have been received via add_addr options.
+	 * The address ID is just the index into the array.
+	 * XXXNJW: This cannot be used for interop, as we cannot
+	 * guarantee a mapping between address IDs and the array indexes
+	 *
+     * *NB: To be removed from the mpcb
+	 * Initialised in mptcp_subr.c
+	 */
+	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*/
+
+	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 */
+	int output_task_pending; /* is a drop task already enqueued */
+
+	int mp_added_address_count;	/* added (via add_addr) subflows */
+	struct sockaddr_storage mp_added_addresses[MAX_ADDRS];
+	struct sockaddr_storage mp_default_address; /* Default interface for conn */
+	struct sockaddr_storage mp_foreign_address; /* Default interface for conn */
+
+	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 from advertised addrs */
+	uint32_t mp_lrnedjoin_mask; /* Mask for MP_JOINs sent to learned addrs */
+
+	/*
+	 * Various async tasks. Used when a task can be performed later, or when
+	 * using a different thread simplifies locking.
+	 *
+	 * XXXNJW: still using the SWI task queue. Want a queue per MPTCP
+	 * connection, and probably a single generic task handler rather than
+	 * specific handlers.
+	 */
+	int mp_event_pending;
+	int mp_join_pending;
+	struct task mp_event_task; /* Receive Dfin, ack of Dfin etc. */
+
+	struct task subflow_event_task; /* Subflows connecting, disconnecting etc. */
+	struct task subflow_detached_task; /* subflow called tp_discardpcb */
+    struct task mp_output_task; /* sbdrop, mapping new data */
+    struct task mp_input_task;  /* sbappend, d-level signalling */
+	struct task	join_task;	/* For enqueuing aysnc joins */
+	struct task subflow_close_task; /* XXXNJW: to ensure subflows usr_close */
+	//	struct task	rexmit_task;	/* For enqueuing data-level rexmits */
+
+	int mp_input_pending;      /* Is an input task already pending */
+	int mp_output_pending;     /* Is an output task already pending */
+	int mp_sf_event_pending;   /* Is a sf event task already pending */
+	int mp_sf_close_pending;   /* Is a sf close task already pending */
+    int mp_sf_detach_pending;  /* Is a sf detach task already pending */
+
+	struct mp_sched mp_temp_sched;
+};
+
+
+/*
+ * A basic, temporary implementation of a list that maps mpcb's to tokens.
+ * This is used to take in incoming join and map the token to an appropriate
+ * mpcb.
+ *
+ * the mpti list, and rwlock, are initialised in mp_init(). an mpcb_tokinfo is
+ * created with each mpcb and inserted into the list at connection established
+ */
+struct mpcb_tokinfo {
+	SLIST_ENTRY(mpcb_tokinfo) mpti_entry;
+    struct mpcb *mpti_pcb;
+    uint32_t mpti_local_token;
+    uint32_t mpti_remote_token;
+};
+
+struct mp_sessions {
+	struct rwlock		 mpti_lock;
+	struct mpti_listhead mpti_listhead;
+};
+
+/* MP closing state machine */
+#define MP_NSTATES 	10			/* Number of MP state machine states */
+#define	MPS_M_CLOSED		0	/* closed */
+#define	MPS_M_ESTABLISHED	1	/* MP session established */
+#define	MPS_M_FIN_WAIT_1	2	/* Sent a D-FIN (active close) */
+#define	MPS_M_FIN_WAIT_2	3   /* D-FIN was ACKED (active close) */
+#define MPS_M_CLOSING		4	/* Simultaneous close */
+#define MPS_M_CLOSE_WAIT	5   /* Received a D-FIN (passive close) */
+#define MPS_M_LAST_ACK		6   /* Sent a D-FIN (passive close) */
+#define MPS_M_TIME_WAIT		7   /* Acked D-FIN (active close) */
+
+/* For mp_flags */
+#define MPF_ACKNOW   0x01
+#define MPF_SENTDFIN 0x02
+
+/* Flags for mp_output, dss_flags (on mtag) */
+#define	MP_DFIN	    0x01
+#define	MP_DACK	    0x02
+#define MP_ADV_ADDR 0x04
+
+#define MPD_DSN32   0x10
+#define MPD_DSN64   0x20
+
+static u_char	mp_outflags[MP_NSTATES] = {
+	0,                  /* 0, MPS_M_CLOSED */
+	0,		            /* 1, MPS_ESTABLISHED */
+	MP_DFIN,            /* 2, MPS_M_FIN_WAIT_1 */
+	MP_DACK,            /* 3, MPS_M_FIN_WAIT_2 */
+	MP_DACK,            /* 4, MPS_M_CLOSING */
+	MP_DACK,            /* 5, MPS_M_CLOSE_WAIT */
+	MP_DFIN,            /* 6, MPS_M_LAST_ACK */
+	MP_DACK,            /* 7, MPS_M_TIME_WAIT */
+	0,
+};
+
+/* States for sf_handle->sf_state */
+/* XXXNJW: actually these should be flags that the MP layer looks at when
+ * dealing with sf_handles, not something the subflow has an awareness of.
+ * E.g. SFS_CLOSING is okay, SFS_SENTCAPABLE,INFINITE_MAP should be in a tp
+ * control block field. */
+#define SFHS_BOUNDSUBFLOW	0x0001  /* Bound, primarily for listen sockets */
+#define SFHS_MPESTABLISHED  0x0002  /* Subflow is using MPTCP */
+#define SFHS_DISCONNECTING  0x0004  /* Commenced protocol disconnect */
+#define SFHS_MPENDED        0x0010  /* No longer capable of using MPTCP */
+
+/* Subflow events flags for tp->t_event_flags. Used to determine what to do
+ * when mp->subflow_event_task_handler runs. */
+#define SFE_CONNECTED        0x0001
+#define SFE_MPESTABLISHED    0x0002
+#define SFE_DISCONNECTING    0x0004
+#define SFE_DISCONNECTED     0x0008
+#define SFE_RCVDDFIN         0x0010
+#define SFE_RCVDDFINACK      0x0020
+#define SFE_RCVDRST          0x0040
+
+/* M-Level events. keep consistent with the subflow events? */
+#define MPE_RCVDDFIN         0x0010
+
+/* XXXNJW: flags set in tcp_input to signal event to be enqueued
+ * at mp-level */
+#define MP_SCHEDINPUT  0x01
+#define MP_SCHEDCLOSE  0x02
+#define MP_SCHEDEVENT  0x04
+#define MP_SCHEDDETACH 0x08
+#define MP_SCHEDJOIN   0x10
+
+/* Subflow flags for for tp->sf_flags . Used to signal input/output behaviours
+ * (such as whether to add certain options etc) */
+#define SFF_GOT_SYNACK		0x00000001
+#define SFF_GOT_JOIN_SYN	0x00000002
+#define SFF_GOT_JOIN_SYNACK	0x00000004
+#define SFF_GOT_JOIN_ACK	0x00000008
+#define	SFF_DATA_WAIT		0x00000010
+#define	SFF_NEED_DACK		0x00000020
+#define	SFF_NEED_DFIN		0x00000040
+#define SFF_SEND_ADD_ADDR 	0x00000080
+#define SFF_SEND_WAIT		0x00000100
+#define SFF_SEND_MPCAPABLE  0x00000200  /* Should add MP_CAPABLE to outgoing */
+#define SFF_SENT_MPCAPABLE	0x00000400	/* Advertised as MP_CAPABLE */
+#define SFF_INFINITEMAP		0x00000800  /* Using infinite map */
+#define SFF_FIRSTSUBFLOW	0x00001000  /* First subflow created */
+#define SFF_LISTENTCPCB    	0x00002000  /* sf is used to accept connections */
+#define SFF_PASSIVE_JOIN    0x00004000
+
+/* States for t_sf_state.  */
+#define SFS_MP_CONNECTING    0x0001
+#define SFS_MP_ENABLED       0x0002
+#define SFS_MP_DISCONNECTING 0x0004
+#define SFS_MP_DISCONNECTED  0x0008
+
+/* 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	1	4
+#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
+#define MP_TOKEN_OFFSET	0
+#define MP_IDSN_OFFSET	12
+
+/* MPTCP signaling 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_TAG_LEN  9
+
+/* DS Map flags */
+#define MAPF_IS_SENT	0x00000001 /* Sent all data from map */
+#define MAPF_IS_ACKED	0x00000002 /* All data in map is acknowledged */
+#define MAPF_IS_DUP		0x00000004 /* Duplicate map, already acked at ds-level */
+#define MAPF_IS_REXMIT	0x00000008 /* Is a rexmit of a previously sent map */
+
+#define SB_NUMMAPPED(mp) 		\
+	((mp)->ds_map_max - (mp)->ds_map_min)
+#define	TCPS_SUBFLOWCLOSING(s)	((s) == TCPS_CLOSE_WAIT)
+#define	intompcb(ip)	(((struct tcpcb *)(ip)->inp_ppcb)->t_mpcb)
+#define	MPS_HAVERCVDFIN(s)	((s) >= MPS_M_CLOSING)
+#define MPS_ENDED(s) ((s) == MPS_M_TIME_WAIT || (s) == MPS_M_CLOSED)
+
+/* XXXNJW: mallocs to be removed */
+MALLOC_DECLARE(M_SFHANDLE); /* Type for subflow handle */
+MALLOC_DECLARE(M_MPTOKINFO); /* Type for subflow handle */
+MALLOC_DECLARE(M_MPTIMERS); /* mp_timers struct */
+MALLOC_DECLARE(M_MPSOPT);   /* mp_sopt struct */
+
+
+/*
+ * MP Options.
+ */
+
+/* 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;
+    uint8_t  dss_flags;
+};
+
+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:
+ * MP Token Info list
+ */
+
+#define MP_LOCK_INIT(mp) 	mtx_init(&mp->mpcb_mutex, "mpcb", NULL, MTX_DEF)
+#define MP_LOCK_DESTROY(mp) 	mtx_destroy(&mp->mpcb_mutex)
+#define MP_LOCK(mp)		mtx_lock(&mp->mpcb_mutex)
+#define MP_UNLOCK(mp)	mtx_unlock(&mp->mpcb_mutex)
+#define	MP_LOCK_ASSERT(mp)	mtx_assert(&mp->mpcb_mutex, MA_OWNED)
+#define MP_UNLOCK_ASSERT(mp) mtx_assert(&mp->mpcb_mutex, MA_UNLOCKED)
+
+/* XXXNJW: Locks for the mp-token list. The list will be replaced with
+ * more substatial at a later time. */
+#define MPTOK_INFO_LOCK_INIT(mpti, d) \
+	rw_init_flags(&(mpti)->mpti_lock, (d), RW_RECURSE)
+#define MPTOK_INFO_LOCK_DESTROY(mpti)  rw_destroy(&(mpti)->mpti_lock)
+#define MPTOK_INFO_RLOCK(mpti)	rw_rlock(&(mpti)->mpti_lock)
+#define MPTOK_INFO_WLOCK(mpti)	rw_wlock(&(mpti)->mpti_lock)
+#define MPTOK_INFO_RUNLOCK(mpti)	rw_runlock(&(mpti)->mpti_lock)
+#define MPTOK_INFO_WUNLOCK(mpti)	rw_wunlock(&(mpti)->mpti_lock)
+#define	MPTOK_INFO_LOCK_ASSERT(mp)	rw_assert(&(mpti)->mpti_lock, RA_WLOCKED)
+
+/* 64-bit DSN comparisons */
+#define	DSEQ_LT(a,b)	((int64_t)((a)-(b)) < 0)
+#define	DSEQ_LEQ(a,b)	((int64_t)((a)-(b)) <= 0)
+#define	DSEQ_GT(a,b)	((int64_t)((a)-(b)) > 0)
+#define	DSEQ_GEQ(a,b)	((int64_t)((a)-(b)) >= 0)
+
+#define	DSEQ_MIN(a, b)	((SEQ_LT(a, b)) ? (a) : (b))
+#define	DSEQ_MAX(a, b)	((SEQ_GT(a, b)) ? (a) : (b))
+
+/* MP-related debug output */
+/*
+ * Keep these defines in sync with debug_class struct in mptcp_subr.c
+	{.class = "MPSESSION"},
+	{.class = "DSMAP"}
+*/
+#define MPSESSION 	0x00000001
+#define DSMAP 		0x00000002
+#define SBSTATUS 	0x00000004
+#define REASS		0x00000008
+
+/* MPTCP systcl identifiers */
+#define	MPTCPCTL_MAXSUBFLOWS	1	/* Maximum subflows allowed */
+#define	MPTCPCTL_SINGLEPKTMAPS	2	/* put DSS map on each packet */
+#define MPTCPCTL_NOTIMEWAIT     3   /* Don't use timeout on M_TIME_WAIT */
+
+VNET_DECLARE(int, max_subflows);
+VNET_DECLARE(int, single_packet_maps);
+
+
+void mpp_init(void);
+void mp_init(void);
+void mp_destroy(void);
+int  mp_newmpcb(struct mppcb *mpp);
+void mp_syncache_newmpcb(struct mpcb *mp, struct syncache *sc);
+void mp_discardcb(struct mpcb *mp);
+struct mpcb*
+     mp_close(struct mpcb *mp);
+struct mpcb*
+     mp_drop(struct mpcb *mp, int error);
+void mp_sf_flush(struct mpcb *mp);
+void mp_mpsopt_flush(struct mpcb *mp);
+struct mp_sopt*
+     mp_alloc_mp_sopt(void);
+int mp_bind_attach(struct socket *so, struct mpcb *mp,
+	struct sockaddr *nam, struct thread *td);
+void mp_appendpkt(struct mbuf *mb, struct mbuf *m_ptr);
+void mp_mbuf_enqueue(struct mpcb *mp, struct mbuf *m);
+void mp_dosubtypes(struct tcpopt *to, uint8_t subtype, u_char *cp,
+	int opt, int optlen, int flags);
+
+/* Debugging stuff */
+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 mpcb *mp);
+int     mp_standard_output(struct mpcb *mp); /* Standard TCP connections */
+int     mp_reass(struct mpcb *mp, struct mbuf *m);
+
+void	ds_reass_init(struct mpcb *m);
+void	mp_order_segment_list(struct tcpcb *tp, struct sockbuf *sb);
+uint64_t mp_generate_local_key(void);
+uint32_t mp_do_hashing(struct mpcb *mp);
+uint32_t mp_do_sha_hash(uint8_t *digest, uint8_t *key, uint64_t key_length);
+uint32_t mp_get_token(uint8_t *digest);
+uint64_t mp_new_idsn(uint8_t *digest);
+u_short  mp_dss_cksum(struct dss_psdhead);
+uint32_t mp_get_hmac (uint8_t *digest, uint64_t local_key, uint64_t remote_key,
+			uint32_t local_rand, uint32_t remote_rand);
+uint32_t
+		mp_get_token(uint8_t *digest);
+
+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 mp_connection *mp_conn, uint64_t local_key);
+void 	mp_process_remote_key(struct mp_connection *mp_conn, uint64_t remote_key);
+void 	mp_state_change(struct mpcb *mp, int state);
+void    mp_syncache_process_local_key(struct syncache *sc);
+void    mp_syncache_process_remote_key(struct syncache *sc,
+            uint64_t remote_key);
+struct mpcb*
+        mp_locate_mpcb(uint32_t token);
+
+void mp_remtoklist(uint32_t local_token); /* free mpti entry */
+
+/* Managing Subflows */
+struct socket *
+        mp_allocghostsocket(struct socket *so);
+void    mp_close_all_subflows(struct mpcb *mp);
+void    mp_reset_all_subflows(struct mpcb *mp);
+int     mp_alloc_subflow_socket(struct socket *so, struct socket **gso);
+int     mp_insert_subflow(struct mpcb *mp, struct socket *sf_so);
+void    mp_enqueue_event(struct mpcb *mp, u_int16_t event_flag);
+void    mp_enqueue_subflow_event(struct tcpcb *tp, u_int16_t event_flag);
+void    mp_subflow_freehandle(struct mpcb *mp, struct sf_handle *sf);
+void    mp_subflow_release_socket(struct socket *so);
+int     mp_attach_subflow(struct socket *so);
+int     mp_create_subflow_implicit(struct mpcb *mp, struct socket *so,
+    struct ip *ip, struct tcphdr *th);
+int     mp_join_respond(struct socket *so, struct tcpcb *tp,
+    struct in_conninfo *inc);
+int  	mp_connect_subflow(struct socket *so, struct sockaddr *nam,
+		    struct thread *td);
+int     mp_create_subflow_socket(struct socket *so, struct socket **sf_gso);
+void    mp_close_subflow_task_handler(void *context, int pending);
+void    mp_sftimewait(struct socket *sf_gso);
+
+/* Socket options */
+int    mp_ctloutput(struct socket *so, struct sockopt *sopt);
+struct mp_sopt* mp_locate_mp_sopt(struct mpcb *mp, struct sockopt *sopt);
+
+/* Interface management */
+int    mp_is_if_up(struct sockaddr *l_addr);
+int    mp_is_addr_default(struct sockaddr_in *l_addr, struct mpcb *mp);
+void   mp_update_available_addresses(struct mpcb *mp);
+
+/* tcp_output related */
+uint32_t
+		mp_get_recwin(struct mpcb *mp);
+void	mp_update_sndwin(struct mpcb *mp, uint16_t win);
+
+struct ds_map *
+		mp_find_dsmap(struct tcpcb *tp, tcp_seq	seqnum);
+
+/* tcp_input related */
+int  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, int acked);
+void mp_init_established(struct mpcb *mp);
+void mp_set_ds_map(struct mpcb *mp, uint64_t ds_num);
+int  mp_do_task_now(struct mpcb *mp, int task_flags);
+
+/* Functions for task queue */
+void mp_schedule_tasks(struct mpcb *mp, int task_flags);
+void mp_join_task_handler(void *context, int pending);
+void mp_subflow_event_task_handler(void *context, int pending);
+void mp_subflow_detached_task_handler(void *context, int pending);
+void mp_output_task_handler(void *context, int pending);
+void mp_input_task_handler(void *context, int pending);
+void mp_drop_task(struct mpcb *mp, int acked);
+int mp_detach_subflow_locked(struct mpcb *mp);
+/* data_level rexmit */
+void    mp_trigger_rexmit(struct mpcb *mp);
+//void    mp_queue_data_rexmit_unlocked(struct mpcb *mp);
+void    mp_queue_data_rexmit_unlocked(struct tcpcb *tp);
+
+#endif /* MPTCP_VAR_H_ */
diff -r 1d1c4c997b66 sys/netinet/tcp.h
--- a/sys/netinet/tcp.h	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp.h	Sun Aug 30 14:27:42 2015 +1000
@@ -97,6 +97,7 @@
 #define    TCPOLEN_TSTAMP_APPA		(TCPOLEN_TIMESTAMP+2) /* appendix A */
 #define	TCPOPT_SIGNATURE	19		/* Keyed MD5: RFC 2385 */
 #define	   TCPOLEN_SIGNATURE		18
+#define TCPOPT_MPTCP	30
 
 /* Miscellaneous constants */
 #define	MAX_SACK_BLKS	6	/* Max # SACK blocks stored at receiver side */
diff -r 1d1c4c997b66 sys/netinet/tcp_input.c
--- a/sys/netinet/tcp_input.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_input.c	Sun Aug 30 14:27:42 2015 +1000
@@ -56,13 +56,16 @@
 #include "opt_ipsec.h"
 #include "opt_tcpdebug.h"
 
+#include <sys/endian.h>
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/hhook.h>
+#include <sys/limits.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
 #include <sys/proc.h>		/* for proc0 declaration */
 #include <sys/protosw.h>
+#include <sys/sbuf.h>
 #include <sys/sdt.h>
 #include <sys/signalvar.h>
 #include <sys/socket.h>
@@ -70,6 +73,12 @@
 #include <sys/sysctl.h>
 #include <sys/syslog.h>
 #include <sys/systm.h>
+#include <sys/kdb.h>
+#include <sys/taskqueue.h>
+
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/limits.h>
 
 #include <machine/cpu.h>	/* before tcp_seq.h, for tcp_random18() */
 
@@ -112,6 +121,13 @@
 #include <netinet/tcp_offload.h>
 #endif
 
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_pcb.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>
@@ -221,6 +237,8 @@
     &VNET_NAME(tcp_autorcvbuf_max), 0,
     "Max size of automatic receive buffer");
 
+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);
@@ -242,14 +260,16 @@
 static void inline	hhook_run_tcp_est_in(struct tcpcb *tp,
 			    struct tcphdr *th, struct tcpopt *to);
 
-/*
- * TCP statistics are stored in an "array" of counter(9)s.
- */
+static int tp_schedule_event(struct tcpcb *tp, int event);
+
+/* TCP statistics are stored in an "array" of counter(9)s. */
 VNET_PCPUSTAT_DEFINE(struct tcpstat, tcpstat);
 VNET_PCPUSTAT_SYSINIT(tcpstat);
 SYSCTL_VNET_PCPUSTAT(_net_inet_tcp, TCPCTL_STATS, stats, struct tcpstat,
     tcpstat, "TCP statistics (struct tcpstat, netinet/tcp_var.h)");
 
+#define MSBIT_CHECK 0x80000000
+
 #ifdef VIMAGE
 VNET_PCPUSTAT_SYSUNINIT(tcpstat);
 #endif /* VIMAGE */
@@ -264,6 +284,7 @@
 	counter_u64_add(VNET(tcpstat)[statnum], 1);
 }
 
+
 /*
  * Wrapper for the TCP established input helper hook.
  */
@@ -610,6 +631,8 @@
 #define	TI_UNLOCKED	1
 #define	TI_WLOCKED	2
 
+	int tried_join_locate = 0;
+
 #ifdef TCPDEBUG
 	/*
 	 * The size of tcp_saveipgen must be the size of the max ip header,
@@ -891,6 +914,60 @@
 		    m->m_pkthdr.rcvif, m);
 #endif /* INET */
 
+	/* Might be a join. rough method for locating the mpcb and creating
+	 * a subflow to handle the incoming. This is used for implicit joins
+	 * (i.e. in cases where an inpcb has not been set up for an address
+	 * pair). Should have a sysctl that controls whether to allow implicit
+	 * joins. */
+	if ((thflags & TH_SYN) && !tried_join_locate) {
+        tcp_dooptions(&to, optp, optlen, TO_SYN);
+
+		/* Returns with mp locked. */
+		if (to.to_mopts.mpo_flags & MPOF_JOIN_SYN) {
+			struct mpcb *mp = NULL;
+
+			/* Might have pulled an inpcb out if joining to the
+			 * address bound as server (would match wildcard src)
+			 *
+			 * XXXNJW: but if the inpcb is pre-allocated, then we
+			 * don't want to override this. need to add a check to
+			 * make sure we don't ignore a valid inpcb and create
+			 * a new one for nothing. (or alternativley, could
+			 * never pre-alloc the inpcb for a subflow).
+			 */
+			if (inp) {
+				INP_WUNLOCK(inp);
+
+				/* XXXNJW a bit wasteful but do an exact lookup.
+				 * if we find an existing inpcb then drop. */
+				inp = in_pcblookup_mbuf(&V_tcbinfo, ip->ip_src,
+					th->th_sport, ip->ip_dst, th->th_dport,
+					INPLOOKUP_WLOCKPCB, m->m_pkthdr.rcvif, m);
+				if(inp) {
+					printf("%s: join on existing tuple\n", __func__);
+					goto dropunlock;
+				}
+			}
+			tried_join_locate = 1;
+			/* Retuns with MP locked */
+			mp = mp_locate_mpcb(to.to_mopts.rcv_token);
+
+			/* if it belongs to an existing mp connection, create a new
+			 * subflow, insert into has list then jump back to findpcb. */
+			if (mp != NULL) {
+				int error_t = 0;
+				/* Drops the mp lock */
+				error_t = mp_create_subflow_implicit(mp,
+					mp->mp_mppcb->mpp_socket, ip, th);
+				if (!error_t)
+					goto findpcb;
+			} else {
+				printf("%s: failed to locate mpcb\n", __func__);
+				goto dropwithreset;
+			}
+		}
+	}
+
 	/*
 	 * If the INPCB does not exist then all data in the incoming
 	 * segment is discarded and an appropriate RST is sent back.
@@ -1038,8 +1115,9 @@
 					goto findpcb;
 				}
 				goto relocked;
-			} else
+			} else {
 				ti_locked = TI_WLOCKED;
+			}
 		}
 		INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
 	}
@@ -1063,6 +1141,210 @@
 		tcp_savetcp = *th;
 	}
 #endif /* TCPDEBUG */
+
+/* MPTCP JOIN */
+	/*
+	 * alternate processing for JOINs (rather than syncache). When we get
+	 * an initial SYN or an ACK of our SYN/ACK
+	 */
+	if ((so->so_options & SO_ACCEPTCONN) &&
+	    (tp->t_sf_flags & SFF_PASSIVE_JOIN)) {
+
+		/* process the options now, as we need to parse the MP_JOIN options */
+		tcp_dooptions(&to, optp, optlen, TO_SYN);
+
+		/* Got an ACK for a SYN/ACK */
+		if (to.to_mopts.mpo_flags & MPOF_JOIN_ACK) {
+			INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
+
+			printf("%s: got ack of join syn/ack\n", __func__);
+
+			so->so_options &= ~SO_ACCEPTCONN;
+
+			/* syncache _socket/_expand would have:
+			 * - marked the socket connected
+			 * - init'd tp->irs
+			 * - transitioned to TCPS_SYN_RECEIVED
+			 * - ??? */
+
+			/*
+			 * Segment validation: the following are taken from syncache_expand
+			 * but adapted to work with a regular tcpcb (as used by passive
+			 * side of mp_join).
+			 */
+
+			/*ACK must match our initial sequence number + 1 (the SYN|ACK). */
+			if (th->th_ack != tp->iss + 1)
+				goto dropwithreset;;
+
+			/* The SEQ must fall in the window starting at the received
+			 * initial receive sequence number + 1 (the SYN). */
+			if (SEQ_LEQ(th->th_seq, tp->irs) ||
+			    SEQ_GT(th->th_seq, tp->irs + tp->rcv_wnd))
+				goto dropwithreset;
+
+			/* If timestamps were not negotiated during SYN/ACK they
+			 * must not appear on any segment during this session. */
+			if (!(tp->t_flags & TF_RCVD_TSTMP) && (to.to_flags & TOF_TS))
+				goto dropwithreset;
+
+			/* The following is based on behaviour of syncache_socket */
+			soisconnected(so);
+			tp->t_state = TCPS_SYN_RECEIVED;
+			tcp_rcvseqinit(tp);
+			tcp_sendseqinit(tp);
+			tp->rcv_adv += tp->rcv_wnd;
+			tp->snd_wl1 = tp->irs;
+			tp->snd_max = tp->iss + 1;
+			tp->snd_nxt = tp->iss + 1;
+			tp->rcv_up = tp->irs + 1;
+			tp->last_ack_sent = tp->rcv_nxt;
+            tp->t_mp_conn.ds_last_dsn = tp->rcv_nxt;
+
+			/* Other timers are activated in e.g. tcp_do_segment, tcp_output */
+			tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
+
+			KASSERT(tp->t_state == TCPS_SYN_RECEIVED,
+				("%s: ", __func__));
+
+			/*
+			 * Process the segment and the data it
+			 * contains.  tcp_do_segment() consumes
+			 * the mbuf chain and unlocks the inpcb.
+			 */
+			tcp_do_segment(m, th, so, tp, drop_hdrlen, tlen,
+				iptos, ti_locked);
+			INP_INFO_UNLOCK_ASSERT(&V_tcbinfo);
+			return (IPPROTO_DONE);
+
+		}
+
+		/* XXXNJW: the following duplicates a lot of code from the
+		 * pre- syncache_add validation of SYN segments. Will need to
+		 * refactor */
+		if (thflags & TH_RST)
+			goto dropunlock;
+
+		/*
+		 * We can't do anything without SYN.
+		 */
+		if ((thflags & TH_SYN) == 0)
+			goto dropunlock;
+
+		/*
+		 * (SYN|ACK) is bogus on a listen socket.
+		 */
+		if (thflags & TH_ACK) {
+			TCPSTAT_INC(tcps_badsyn);
+			rstreason = BANDLIM_RST_OPENPORT;
+			goto dropwithreset;
+		}
+
+		KASSERT((thflags & (TH_RST|TH_ACK)) == 0,
+			("%s: Listen socket: TH_RST or TH_ACK set", __func__));
+		KASSERT(thflags & (TH_SYN),
+			("%s: Listen socket: TH_SYN not set", __func__));
+
+		if (th->th_dport == th->th_sport &&
+			ip->ip_dst.s_addr == ip->ip_src.s_addr) {
+//			if ((s = tcp_log_addrs(&inc, th, NULL, NULL)))
+//				log(LOG_DEBUG, "%s; %s: Listen socket: "
+//				"Connection attempt from/to self "
+//				"ignored\n", s, __func__);
+			goto dropunlock;
+		}
+		if (IN_MULTICAST(ntohl(ip->ip_dst.s_addr)) ||
+			IN_MULTICAST(ntohl(ip->ip_src.s_addr)) ||
+			ip->ip_src.s_addr == htonl(INADDR_BROADCAST) ||
+			in_broadcast(ip->ip_dst, m->m_pkthdr.rcvif)) {
+//			if ((s = tcp_log_addrs(&inc, th, NULL, NULL)))
+//				log(LOG_DEBUG, "%s; %s: Listen socket: "
+//				"Connection attempt from/to broad- "
+//				"or multicast address ignored\n",
+//				s, __func__);
+			goto dropunlock;
+		}
+
+		/*
+		 * 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) {
+			struct in_conninfo inc;
+            int error = 0;
+
+			KASSERT(tp->t_state == TCPS_LISTEN, ("%s: so accepting but "
+				"tp not listening", __func__));
+			bzero(&inc, sizeof(inc));
+			{
+			    inc.inc_faddr.s_addr = ip->ip_src.s_addr;
+			    inc.inc_laddr.s_addr = ip->ip_dst.s_addr;
+			}
+			inc.inc_fport = th->th_sport;
+			inc.inc_lport = th->th_dport;
+			inc.inc_fibnum = so->so_fibnum;
+
+			mp_debug(MPSESSION, 2, 0, "%s: got JOIN_SYN\n", __func__);
+			/*
+			 * Got a MP_JOIN_SYN option. Need to send a JOIN_SYNACK
+			 * in reponse.
+			 */
+			if (to.to_mopts.optlen != MPTCP_SUBLEN_MP_JOIN_SYN) {
+				to.to_mopts.mpo_flags &= ~MPOF_MP_JOIN;
+				goto dropwithreset;
+			}
+
+			tp->snd_wnd = th->th_win;
+			if (to.to_flags & TOF_TS) {
+			    tp->t_flags |= TF_RCVD_TSTMP;
+			    tp->ts_recent = to.to_tsval;
+			    tp->ts_recent_age = tcp_ts_getticks();
+			}
+
+            if (to.to_flags & TOF_MSS)
+            	tcp_mss(tp, to.to_mss);
+        	if ((tp->t_flags & TF_SACK_PERMIT) &&
+        	    (to.to_flags & TOF_SACKPERM) == 0)
+        		tp->t_flags &= ~TF_SACK_PERMIT;
+
+			if (!(to.to_flags & TOF_SACK))
+				tp->t_flags &= ~TF_SACK_PERMIT;
+
+			if ((to.to_flags & TOF_SCALE) && (tp->t_flags & TF_REQ_SCALE)) {
+				int wscale = 0;
+			    tp->t_flags |= TF_RCVD_SCALE;
+			    tp->snd_scale = to.to_wscale;
+
+			    while (wscale < TCP_MAX_WINSHIFT &&
+			        (TCP_MAXWIN << wscale) < sb_max)
+			    	    wscale++;
+			    tp->request_r_scale = wscale;
+			}
+
+			tp->t_mp_conn.remote_rand = to.to_mopts.snd_rnd;
+			tp->t_mp_conn.mp_conn_token = to.to_mopts.rcv_token; // might not need to keep this
+            tp->t_mp_conn.local_rand = arc4random();
+
+            tp->t_sf_state |= SFS_MP_CONNECTING;
+			tp->t_sf_flags |= SFF_GOT_JOIN_SYN;
+			error = mp_join_respond(so, tp, &inc);
+            if (error)
+            	printf("%s: error %d\n", __func__, error);
+
+			if (ti_locked == TI_WLOCKED) {
+				INP_INFO_WUNLOCK(&V_tcbinfo);
+				ti_locked = TI_UNLOCKED;
+			}
+			INP_INFO_UNLOCK_ASSERT(&V_tcbinfo);
+			return (IPPROTO_DONE);
+		}
+
+		/* XXXNJW: think more about what to do in this case. */
+		goto dropwithreset;
+	}
+/* MPTCP JOIN end */
+
 	/*
 	 * When the socket is accepting connections (the INPCB is in LISTEN
 	 * state) we look into the SYN cache if this is a new connection
@@ -1152,6 +1434,7 @@
 			tp = intotcpcb(inp);
 			KASSERT(tp->t_state == TCPS_SYN_RECEIVED,
 			    ("%s: ", __func__));
+
 #ifdef TCP_SIGNATURE
 			if (sig_checked == 0)  {
 				tcp_dooptions(&to, optp, optlen,
@@ -1182,6 +1465,8 @@
 			INP_INFO_UNLOCK_ASSERT(&V_tcbinfo);
 			return (IPPROTO_DONE);
 		}
+		/* end syncache check on ACK */
+
 		/*
 		 * Segment flag validation for new connection attempts:
 		 *
@@ -1489,6 +1774,13 @@
 	struct in_conninfo *inc;
 	struct mbuf *mfree;
 	struct tcpopt to;
+	struct ds_map* map = NULL;
+
+	/* MPTCP */
+	struct mbuf *m_ptr = NULL;
+    struct mpcb *mp = tp->t_mpcb;
+    int mp_wakeup = 0;
+    mpp_pcbref(mp->mp_mppcb);
 
 #ifdef TCPDEBUG
 	/*
@@ -1612,12 +1904,334 @@
 		}
 	}
 
+    /* Header contains MPTCP options */
+	/* Currently double up on mbufs that are enqueued if
+	 * we get signaling and some data - i.e. the mbuf will
+	 * be copied with the header, and the segment itself
+	 * will be inserted throught the reass process. Though
+	 * this double processes the same mbuf, it allows
+	 * in-order messages to be passed to the mp-layer even
+	 * if the asscociated data is out-of-order at the
+	 * subflow level. */
+	if ((to.to_flags & TOF_MPTCP)) {
+
+		if (to.to_mopts.mpo_flags &
+		    (MPOF_DATA_ACK | MPOF_ADD_ADDR_V4 | MPOF_ADD_ADDR_V6)) {
+
+			if (to.to_mopts.mpo_flags & MPOF_ADD_ADDR_V4)
+				printf("%s: got add_addr\n", __func__);
+
+			// we have a DACK, need to copy the mbuf to
+			// pass to the mp-layer.
+			// a DACK on a segment carrying data means we
+			// end up with two copies of the segment. (one
+			// with whole segment, one with dropped header
+			// and mbuf tag).
+			if (m_ptr == NULL)
+				m_ptr =  m_copypacket(m, M_NOWAIT);
+			else
+			    mp_appendpkt(m_ptr, m_copypacket(m, M_NOWAIT));
+		}
+
+		/* XXXNJW: Got a d-fin. The adjustment for the dfin happens in
+		 * another thread, need to test to make sure this all works okay
+		 * if there happens to also be data on the segment as well. */
+		if (to.to_mopts.mpo_flags & MPOF_DATA_FIN) {
+			/* DFIN only, without any other data. */
+			if (to.to_mopts.sub_seq_num == 0) {
+				struct dsn_tag *dtag = (struct dsn_tag *)
+					m_tag_alloc(PACKET_COOKIE_MPTCP, PACKET_TAG_DSN,
+						DSN_TAG_LEN, M_NOWAIT); // space for 64-bit DSN
+				struct m_tag *mtag = &dtag->tag;
+
+				dtag->dsn = to.to_mopts.data_seq_num;
+				dtag->dss_flags |= MP_DFIN;
+				m_tag_prepend(m, mtag);
+
+				/* get rid of the tcp/ip headers */
+				m_adj(m, drop_hdrlen);
+
+				/* queue up for processing. in mp_input this segment
+				 * will be  */
+				if (m_ptr == NULL)
+					m_ptr = m_copypacket(m, M_NOWAIT);
+				else
+				    mp_appendpkt(m_ptr, m_copypacket(m, M_NOWAIT));
+
+			}
+
+			/* If there's other data attached, then it will go
+			 * via reass. */
+		}
+
+		/*
+		 * Do we need to send a data-level ACK in response
+		 * to this segment?
+		 * XXXNJW: related to MP_JOINs, but might not
+		 * need to use this (could check for the join
+		 * option flag).
+		 */
+		if (to.to_mopts.mpo_flags & MPOF_NEED_ACK)
+			tp->t_flags |= TF_ACKNOW;
+
+		/* For MP_JOINs on existing MP sessions */
+		/* XXXNJW: Should not need the check here for FIRSTSUBFLOW - use the
+		 * MP_JOIN or MP_CAPABLE options to figure this out.*/
+		if ((tp->t_sf_state & SFS_MP_CONNECTING) &&
+			(to.to_mopts.mpo_flags & MPOF_JOIN_ACK)) {
+			if (to.to_mopts.optlen == MPTCP_SUBLEN_MP_JOIN_ACK) {
+				// add the join stuff back in... which would be?
+			}
+		} /* End join processing */
+
+		/*
+		 * Got a new DSS mapping for this subflow.
+		 */
+		if ((to.to_mopts.mpo_flags & MPOF_DSS)) {
+
+			tp->t_mp_conn.ds_last_dsn = to.to_mopts.data_seq_num;
+
+			if ((to.to_mopts.mpo_flags & MPOF_DSN_MAP) &&
+				!((to.to_mopts.mpo_flags & MPOF_DATA_FIN)
+				&& to.to_mopts.dss_data_len == 1)) {
+
+				/* got a DSN with no length, drop to infinite mapping */
+				if (to.to_mopts.dss_data_len == 0)
+					panic("dropping back to infinite map not implemented\n");
+
+				struct ds_map *temp_map, *temp_map2, *next_map = NULL;
+				int found_dup_map = 0;
+
+				/* find the map that this segment belongs to */
+				TAILQ_FOREACH_SAFE(temp_map, &tp->t_rcv_maps.dsmap_list,
+						sf_ds_map_next, temp_map2) {
+
+					/* Found a completed map, so remove it from t_rxdmaps */
+					if (SEQ_GEQ(tp->rcv_nxt,temp_map->sf_seq_start
+						+ temp_map->ds_map_len)) {
+						mp_debug(DSMAP, 4, 0, "remove map: sf_seq_start: %u. "
+								"end: %u\n", temp_map->sf_seq_start,
+								temp_map->sf_seq_start + temp_map->ds_map_len);
+						TAILQ_REMOVE(&tp->t_rcv_maps.dsmap_list, temp_map,
+							sf_ds_map_next);
+						free(temp_map, M_DSSMAP);
+						continue;
+					}
+
+					if (!next_map) {
+						int map_offset = 0;
+						tcp_seq new_sf_map_start =
+							to.to_mopts.sub_seq_num + tp->irs;
+						tcp_seq temp_map_end =
+							temp_map->sf_seq_start + temp_map->ds_map_len;
+						map_offset = new_sf_map_start - temp_map->sf_seq_start;
+
+						/* A segment part-way into an already recv'd mapping? */
+	//					if (new_sf_map_start > temp_map->sf_seq_start &&
+	//						to.to_mopts.data_seq_num == temp_map->ds_map_start) {
+						// XXXNJW this would be a broken mapping
+						// (SSN, DSN) out of sync
+
+						/* is this map fully covered by an existing map? If yes
+						 * then it doesn't tell us anything we don't already know,
+						 * use the existing map from here on.
+						 */
+						if (new_sf_map_start >= temp_map->sf_seq_start &&
+							(to.to_mopts.dss_data_len + map_offset <=
+							temp_map->ds_map_len)) {
+	//
+	//						/* double check that DSNs are aligned, if so set dup */
+	//						if (temp_map->ds_map_start + map_offset ==
+	//						    to.to_mopts.data_seq_num) {
+	//							found_dup_map = 1;
+	//							map = temp_map;
+	//						}
+							found_dup_map = 1;
+							map = temp_map;
+
+						}
+
+						/* An unseen map, therefore should be inserted before
+						 * temp_map in our map list
+						 *
+						 * Some wrap detection: the 0xC000... is a simple (too
+						 * simple?) test to see if the new map occurs after a wrap.
+						 * Included edge case where map ends exactly on the end of
+						 * the sequence space.
+						 */
+						if ((new_sf_map_start > temp_map_end &&
+							(temp_map_end > temp_map->sf_seq_start ||
+							(new_sf_map_start & 0xC0000000) == 0)) ||
+							(temp_map_end == UINT_MAX && new_sf_map_start == 0)) {
+							next_map = temp_map;
+						}
+
+					}
+
+				}
+
+				/*
+				 * If we do not find a map covered by this sequence number,
+				 * allocate a new map and put into mapping list.
+				 * Also for cases where !has_dsn_map (map will be NULL
+				 * in this case)
+				 */
+				if (!found_dup_map) {
+
+					/* XXX: should use zone allocator */
+					map = malloc(sizeof(struct ds_map), M_DSSMAP, M_NOWAIT);
+
+					/* set values for mapping */
+					map->ds_map_start = to.to_mopts.data_seq_num;
+					map->sf_seq_start = to.to_mopts.sub_seq_num + tp->irs;
+					map->ds_map_len = map->ds_map_offset =
+						to.to_mopts.dss_data_len;
+					map->ds_map_csum = to.to_mopts.dss_csum;
+
+					mp_debug(DSMAP, 4, 0, "%s: inserting map sf_seq_start %u, "
+						"end %u seq: %u\n", __func__, map->sf_seq_start,
+						map->sf_seq_start + map->ds_map_len, th->th_seq);
+
+					/* if there are no existing maps insert at head. Other
+					 * wise insert in sequence */
+					if (next_map == NULL) {
+	//					KASSERT(TAILQ_EMPTY(&tp->t_rxdsmaps),
+	//						("Incoming sequence %u less than those in rx maps\n"));
+	//					TAILQ_INSERT_HEAD(&tp->t_rxdsmaps, map, sf_ds_map_next);
+						if (TAILQ_EMPTY(&tp->t_rcv_maps.dsmap_list) ||
+							map->sf_seq_start == tp->rcv_nxt) {
+							TAILQ_INSERT_TAIL(&tp->t_rcv_maps.dsmap_list, map,
+								sf_ds_map_next);
+						} else {
+							goto dropafterack;
+						}
+					} else {
+						TAILQ_INSERT_BEFORE(next_map, map, sf_ds_map_next);
+					}
+				}
+			}
+		} /* End DSS Map processing */
+	} /* TOF_MPTCP */
+
+	/* Does the incoming segment have len beyond the headers?
+	 * If so, need to apply a dsn tag to it, regardless of if
+	 * the segment came with a dsn option, or whether we are
+	 * in MP or standard TCP modes. */
+	if (tlen) {
+		/* XXXNJW - Handling RST segments with payloads (reset cause)
+		 * skip processing for RST payloads? A RST on a single
+		 * subflow might include some payload explaining the RST
+		 * cause. Consider this as not part of the data-level
+		 * stream so should not attempt to reassemble, as it will
+		 * break the data-level mappings.
+		 */
+
+		/* 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_TAG_LEN,
+		        M_NOWAIT); // space for 64-bit DSN
+		struct m_tag *mtag = &dtag->tag;
+
+		/*
+		 * 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)) {
+			 dtag->dss_flags |= MPD_DSN32;
+		}
+
+		/* XXNJW: TODO
+		 * In multi-packet map scenario, the packet carrying the map
+		 * might be lost. Thus packets arriving after this will not
+		 * belong to an existing map. These packets are buffered while
+		 * we wait for the packet with the map to be retransmitted.
+		 * If the zone for buffering packets is exhausted, we simply send
+		 * an ACK and drop the segment, making the sender retransmit.
+		 * Otherwise we insert the segment into the list.
+		 */
+
+		// Could potentially expand the dtag to take both the sqn
+		// and dsn for each segment. The dsn will be non-valid for
+		// standard TCP connections but at least the subflow won't
+		// need to know about whether MP is enabled (i.e. always
+		// behave the same).
+
+		if ((tp->t_sf_state & SFS_MP_ENABLED) == 0) {
+//			uint64_t t_dsn;
+//			t_dsn = tp->t_mp_conn.ds_last_dsn
+//				+ tp->t_mp_conn.bytes_since_last_dsn;
+//			tp->t_mp_conn.bytes_since_last_dsn += tlen;
+			dtag->dsn = th->th_seq;
+		} else {
+			if (map == NULL) {
+				/* find the map that this segment belongs to. First map that
+				 * incoming subflow seq num is larger than. This is for cases
+				 * where  */
+				TAILQ_FOREACH(map, &tp->t_rcv_maps.dsmap_list, sf_ds_map_next) {
+					/*
+					 * XXXNJW: The following panic needs to be changed to handle
+					 * the buffering of packets beyond the mapped area (for cases where
+					 * the DSN-carrying packet has been lost, or arrives out of order.
+					 *
+					 * XXXNJW: Collapse this into a single block once verified
+					 */
+//					if (map->sf_seq_start + map->ds_map_len > map->sf_seq_start) {
+//						if (th->th_seq > map->sf_seq_start + map->ds_map_len)
+//							panic("%s: Received data beyond end of mapped region."
+//							"ssn: %u, mapstart %u, maplen %u\n",
+//							__func__, th->th_seq, map->sf_seq_start, map->ds_map_len);
+//					} else {
+//						if ((th->th_seq > map->sf_seq_start + map->ds_map_len) &&
+//						    (th->th_seq < map->sf_seq_start))
+//							panic("%s: Received data beyond end of mapped region."
+//							"ssn: %u, mapstart %u, maplen %u\n",
+//							__func__, th->th_seq, map->sf_seq_start, map->ds_map_len);
+//					}
+
+					uint32_t seqnum = th->th_seq;
+					uint32_t sf_seq_end = map->sf_seq_start + map->ds_map_len;
+					if ((seqnum >= map->sf_seq_start && sf_seq_end > map->sf_seq_start
+						&& seqnum < sf_seq_end) ||
+						(sf_seq_end < map->sf_seq_start && seqnum < map->sf_seq_start
+						&& seqnum < sf_seq_end) ||
+						(sf_seq_end < map->sf_seq_start && seqnum >= map->sf_seq_start
+						&& seqnum <= UINT_MAX)) {
+							/* A map exists for this packet */
+							mp_debug(DSMAP, 4, 0, "found map: ds_map_start %ju, "
+							"sd_seq_start: %u. len: %d\n", map->ds_map_start,
+							map->sf_seq_start, map->ds_map_len);
+							break;
+					}
+				}
+			}
+
+			/* Must have a valid map beyond this point */
+			KASSERT(map != NULL, ("%s:%d No map found for packet with "
+			    "sqn %u\n", __func__, __LINE__, th->th_seq));
+
+			/* update mbuf tag with current data seq num */
+			dtag->dsn = map->ds_map_start + (th->th_seq - map->sf_seq_start);
+//			printf("%s: sub dsn %u\n", __func__, (uint32_t) th->th_seq);
+//			printf("%s: map dsn %ju\n", __func__, dtag->dsn);
+		}
+
+		/* Set D-Fin flag if this DSS had a dfin on it. */
+		if (to.to_mopts.mpo_flags & MPOF_DATA_FIN)
+		    dtag->dss_flags |= MP_DFIN;
+
+		/* And prepend the mtag to mbuf, to be checked in reass */
+		m_tag_prepend(m, mtag);
+	} /* End tlen > 0 */
+
 	/*
 	 * Process options only when we get SYN/ACK back. The SYN case
 	 * for incoming connections is handled in tcp_syncache.
 	 * According to RFC1323 the window field in a SYN (i.e., a <SYN>
 	 * or <SYN,ACK>) segment itself is never scaled.
-	 * XXX this is traditional behavior, may need to be cleaned up.
+	 * XXX this is traditional behaviour, may need to be cleaned up.
 	 */
 	if (tp->t_state == TCPS_SYN_SENT && (thflags & TH_SYN)) {
 		if ((to.to_flags & TOF_SCALE) &&
@@ -1640,7 +2254,66 @@
 		if ((tp->t_flags & TF_SACK_PERMIT) &&
 		    (to.to_flags & TOF_SACKPERM) == 0)
 			tp->t_flags &= ~TF_SACK_PERMIT;
-	}
+
+		/* MP-options only used during connection setup. */
+		if (to.to_flags & TOF_MPTCP) {
+			if (tp->t_sf_flags & SFF_SENT_MPCAPABLE) {
+				mp_process_remote_key(&tp->t_mp_conn,
+					to.to_mopts.remote_key);
+				/* send mp_cabable ACK, got SYN/ACK. NOTE this is
+				 * different to JOIN_SYNACK */
+				tp->t_sf_flags |= SFF_GOT_SYNACK;
+				/* enable csum if remote end sets csum bit */
+				if (to.to_mopts.mpo_flags & MPOF_USE_CSUM)
+					tp->t_mpcb->csum_enabled = 1;
+			} else {
+				/* got a MP_CAPABLE but did not send this option */
+				// XXXNJW: If we did not send an MP_CAPABLE, the
+				// response should not include the option. Either
+				// drop the segment or just ignore and continue on
+				// with standard TCP.
+			}
+
+			/* could be a subflow SYN/ACK */
+			if (to.to_mopts.mpo_flags & MPOF_JOIN_SYNACK) {
+				if (to.to_mopts.mpo_flags & MPOF_BACKUP_PATH) {
+					// XXXNJW tbd: this flow is a backup path
+					// should drop the subflow rather than panic the
+					// host
+					panic("Backup subflows not supported yet\n");
+				}
+
+				tp->t_mp_conn.hmac_trun_remote = to.to_mopts.snd_trc_mac;
+				tp->t_mp_conn.remote_rand = to.to_mopts.snd_rnd;
+
+				/* Calculate a MAC to return in the MP_JOIN ACK using
+				 * the random number just rcvd, and the keys from the
+				 * initial mp establishment
+				 */
+
+				/* MAC-A */
+				uint32_t mac_len = mp_get_hmac(tp->t_mp_conn.hmac_local,
+						tp->t_mp_conn.local_key, tp->t_mp_conn.remote_key,
+						tp->t_mp_conn.local_rand,
+						tp->t_mp_conn.remote_rand);
+
+				if (mac_len == 0) {
+					mp_debug(MPSESSION, 4, 0,"%s: error computing MAC-A\n",
+						__func__);
+					goto dropwithreset;
+				}
+
+				// XXXNJW: Need to validate the HMACs before continuing
+				// i.e. calculate the remote hmac and compare the leftmost
+				// 64 bits with the those in the truncated HMAC
+
+				tp->t_sf_flags |= SFF_GOT_JOIN_SYNACK;
+				mp_debug(MPSESSION, 4, 0,"%s: got join syn/ack\n",
+					__func__);
+			}
+		}
+
+	} /* End SYN/ACK processing */
 
 	/*
 	 * Header prediction: check for the two common cases
@@ -1740,7 +2413,7 @@
 				if (SEQ_GT(tp->snd_una, tp->snd_recover) &&
 				    SEQ_LEQ(th->th_ack, tp->snd_recover))
 					tp->snd_recover = th->th_ack - 1;
-				
+
 				/*
 				 * Let the congestion control algorithm update
 				 * congestion control related information. This
@@ -1749,7 +2422,14 @@
 				 */
 				cc_ack_received(tp, th, CC_ACK);
 
+				/* Drop data from the send map */
+				dsmap_drop(&tp->t_send_maps.dsmap_list, th->th_ack, acked);
+
 				tp->snd_una = th->th_ack;
+
+				/* process ack at the mp-layer */
+				if ((tp->t_sf_state & SFS_MP_ENABLED) == 0)
+                    mp_wakeup |= MP_SCHEDINPUT;
 				/*
 				 * Pull snd_wl2 up to prevent seq wrap relative
 				 * to th_ack.
@@ -1779,11 +2459,16 @@
 				else if (!tcp_timer_active(tp, TT_PERSIST))
 					tcp_timer_activate(tp, TT_REXMT,
 						      tp->t_rxtcur);
+                /* XXXNJW Is there stuff to send? Only if we have a map already
+                 * allocated. Otherwise sowwakeup will cause the map allocator
+                 * to run if there is new data that can be sent.
+                 */
 				sowwakeup(so);
 				if (sbavail(&so->so_snd))
 					(void) tcp_output(tp);
 				goto check_delack;
 			}
+			/* End pure ACK processing */
 		} else if (th->th_ack == tp->snd_una &&
 		    tlen <= sbspace(&so->so_rcv)) {
 			int newsize = 0;	/* automatic sockbuf scaling */
@@ -1801,9 +2486,9 @@
 			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
+//			tp->rcv_nxt += tlen;									// will cause accounting error
+			/*                                                      // as previously were appending
+			 * Pull snd_wl1 up to prevent seq wrap relative to      // directly from here.
 			 * th_seq.
 			 */
 			tp->snd_wl1 = th->th_seq;
@@ -1811,9 +2496,9 @@
 			 * Pull rcv_up up to prevent seq wrap relative to
 			 * rcv_nxt.
 			 */
-			tp->rcv_up = tp->rcv_nxt;
-			TCPSTAT_INC(tcps_rcvpack);
-			TCPSTAT_ADD(tcps_rcvbyte, tlen);
+			tp->rcv_up += tlen;									// as we don't increment rcv_nxt above
+			TCPSTAT_INC(tcps_rcvpack);                          // just increment directly by tlen
+			TCPSTAT_ADD(tcps_rcvbyte, tlen);                    // rcv_nxt will be bumped in tcp_reass below
 			ND6_HINT(tp);	/* Some progress has been made */
 #ifdef TCPDEBUG
 			if (so->so_options & SO_DEBUG)
@@ -1877,23 +2562,34 @@
 			}
 
 			/* Add data to socket buffer. */
-			SOCKBUF_LOCK(&so->so_rcv);
-			if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
-				m_freem(m);
-			} else {
-				/*
-				 * Set new socket buffer size.
-				 * Give up when limit is reached.
+//			if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
+//				m_freem(m);
+//			} else {
+				/* XXXNJW: put the data into the subflow reass list. Don't
+				 * do a fastpath append as in standard implementation, though
+				 * could eventually put in a shortcut to adjust the subflow
+				 * accounting here and append directly to the mp-level list
+				 *
+				 * Should call tcp_reass and not do an append of the data
+				 * to the socket buffer. The wakeup that is usually here has
+				 * also been removed (wakeup will be triggered if we end up
+				 * appending segments at the data-level.
 				 */
-				if (newsize)
-					if (!sbreserve_locked(&so->so_rcv,
-					    newsize, so, NULL))
-						so->so_rcv.sb_flags &= ~SB_AUTOSIZE;
-				m_adj(m, drop_hdrlen);	/* delayed header drop */
-				sbappendstream_locked(&so->so_rcv, m, 0);
-			}
+                m_adj(m, drop_hdrlen);	/* delayed header drop */
+				thflags = tcp_reass(tp, th, &tlen, m);
+
+				/* is there any new in-order data? */
+				if (tp->t_segq_received) {
+//					printf("%s:%d received seg sqn %u\n", __func__, __LINE__,
+//						th->th_seq);
+					if (m_ptr == NULL)
+						m_ptr = tp->t_segq_received;
+					else
+						mp_appendpkt(m_ptr, tp->t_segq_received);
+					tp->t_segq_received = NULL;
+				}
+//			}
 			/* NB: sorwakeup_locked() does an implicit unlock. */
-			sorwakeup_locked(so);
 			if (DELAY_ACK(tp, tlen)) {
 				tp->t_flags |= TF_DELACK;
 			} else {
@@ -1902,13 +2598,17 @@
 			}
 			goto check_delack;
 		}
-	}
+	} /* End fast path (TCPS_ESTABLISHED) */
 
 	/*
 	 * Calculate amount of space in receive window,
 	 * and then do TCP input processing.
 	 * Receive window is amount of space in rcv queue,
 	 * but not less than advertised window.
+	 *
+	 * XXXNJW: When mptcp is active, want to use the
+	 * connection-level receive window, stored in the
+	 * mpcb.
 	 */
 	win = sbspace(&so->so_rcv);
 	if (win < 0)
@@ -1923,7 +2623,7 @@
 
 	/*
 	 * If the state is SYN_RECEIVED:
-	 *	if seg contains an ACK, but not for our SYN/ACK, send a RST.
+	 * if seg contains an ACK, but not for our SYN/ACK, send a RST.
 	 */
 	case TCPS_SYN_RECEIVED:
 		if ((thflags & TH_ACK) &&
@@ -1932,6 +2632,7 @@
 				rstreason = BANDLIM_RST_OPENPORT;
 				goto dropwithreset;
 		}
+		tp->snd_una++;
 		break;
 
 	/*
@@ -1965,11 +2666,23 @@
 		if (!(thflags & TH_SYN))
 			goto drop;
 
+		/* Set initial receive values for tpcb and mpcb. initially we
+		 * are in infinite mapping mode so both set from tp->irs.
+		 * mp protocol specifies increment by 1. */
 		tp->irs = th->th_seq;
 		tcp_rcvseqinit(tp);
+        tp->t_mp_conn.ds_last_dsn = tp->rcv_nxt;
+//		if (tp->t_sf_flags & SFF_FIRSTSUBFLOW) {
+//			MP_INNER_LOCK(tp->t_mpcb);
+//			mp_rcvseqinit(tp->t_mpcb, tp);
+//			MP_INNER_UNLOCK(tp->t_mpcb);
+//		}
+
 		if (thflags & TH_ACK) {
 			TCPSTAT_INC(tcps_connects);
 			soisconnected(so);
+			printf("%s: connected tp %p so %p\n", __func__, tp, so);
+
 #ifdef MAC
 			mac_socketpeer_set_from_mbuf(m, so);
 #endif
@@ -1981,6 +2694,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.
@@ -2014,6 +2728,28 @@
 				cc_conn_init(tp);
 				tcp_timer_activate(tp, TT_KEEP,
 				    TP_KEEPIDLE(tp));
+
+				/* MPTCP */
+				if (to.to_flags & TOF_MPTCP) {
+					/* Have also become an MPTCP connection. Let the mp
+					 * control layer know. */
+					if(tp->t_sf_flags & SFF_SENT_MPCAPABLE) {
+						tp->t_sf_flags &= ~SFF_INFINITEMAP;
+						tp->t_sf_state |= SFS_MP_ENABLED;
+						mp_wakeup |= tp_schedule_event(tp, SFE_MPESTABLISHED);
+					} else if(tp->t_sf_state & SFS_MP_CONNECTING) {
+						tp->t_sf_state &= ~SFS_MP_CONNECTING;
+						tp->t_sf_state |= SFS_MP_ENABLED;
+						mp_debug(MPSESSION, 1, 0,
+						    "%s:%d new mp connection for tp: %p: \n", __func__,
+						    __LINE__, tp);
+						mp_debug(MPSESSION, 4, 0,
+							"%s: set subflow as mp_active\n", __func__);
+					}
+				}
+				/* Let the mp control layer know that a subflow has
+				 * reached a connected state. */
+				mp_wakeup |= tp_schedule_event(tp, SFE_CONNECTED);
 			}
 		} else {
 			/*
@@ -2062,6 +2798,8 @@
 
 		goto step6;
 
+	/* Case SYN_SENT */
+
 	/*
 	 * If the state is LAST_ACK or CLOSING or TIME_WAIT:
 	 *      do normal processing.
@@ -2072,6 +2810,8 @@
 	case TCPS_CLOSING:
 		break;  /* continue normal processing */
 	}
+	/* End SYN/LISTEN processing */
+
 
 	/*
 	 * States other than LISTEN or SYN_SENT.
@@ -2089,6 +2829,8 @@
 	 * drop leading data (and SYN); if nothing left, just ack.
 	 */
 	if (thflags & TH_RST) {
+		printf("%s: got RST, tseq %u last_ack %u rcv_nxt %u\n", __func__,
+			th->th_seq, tp->last_ack_sent, tp->rcv_nxt);
 		/*
 		 * RFC5961 Section 3.2
 		 *
@@ -2129,9 +2871,18 @@
 					tcp_state_change(tp, TCPS_CLOSED);
 					/* FALLTHROUGH */
 				default:
+					printf("%s: close after RST\n", __func__);
 					tp = tcp_close(tp);
+
+					/* XXXNJW: This will ensure that a close is
+					 * called on the subflow socket, as the wakeups
+					 * (in tcp_drop) won't actually cause a close in this
+					 * case. */
+					if (tp->t_sf_state & SFS_MP_ENABLED)
+                        mp_wakeup |= MP_SCHEDCLOSE;
 				}
 			} else {
+				printf("%s: send RST challenge\n", __func__);
 				TCPSTAT_INC(tcps_badrst);
 				/* Send challenge ACK. */
 				tcp_respond(tp, mtod(m, void *), th, m,
@@ -2167,6 +2918,8 @@
 		}
 		goto drop;
 	}
+	/* End RST Processing */
+
 
 	/*
 	 * RFC 1323 PAWS: If we have a timestamp reply on this segment
@@ -2211,6 +2964,7 @@
 		goto dropwithreset;
 	}
 
+	/* Trimming to fit rcv window */
 	todrop = tp->rcv_nxt - th->th_seq;
 	if (todrop > 0) {
 		if (thflags & TH_SYN) {
@@ -2249,13 +3003,23 @@
 		drop_hdrlen += todrop;	/* drop from the top afterwards */
 		th->th_seq += todrop;
 		tlen -= todrop;
+
+		/* dsn should be incremented by the same length as th_seq */
+		if (map) {
+			struct m_tag *mtag =  m_tag_locate(m, PACKET_COOKIE_MPTCP,
+			    PACKET_TAG_DSN, NULL);
+			KASSERT(mtag != NULL, ("%s segment %u missing an mbuf tag\n",
+			    __func__, th->th_seq - todrop));
+			((struct dsn_tag *)mtag)->dsn += todrop;
+		}
+
 		if (th->th_urp > todrop)
 			th->th_urp -= todrop;
 		else {
 			thflags &= ~TH_URG;
 			th->th_urp = 0;
 		}
-	}
+	} /* end segment trimming, seq < rcv_nxt */
 
 	/*
 	 * If new data are received on a connection after the
@@ -2303,6 +3067,14 @@
 				goto dropafterack;
 		} else
 			TCPSTAT_ADD(tcps_rcvbyteafterwin, todrop);
+
+		//XXXNJW: Were issues with trimming the end of an incoming segment
+		// that would mess up the dsn/ssn mappings and cause a stall. This
+		// might have been fixed so should try removing this and testing.
+		// So what happens now is that segments that are trimmed due to having
+		// length past the rcv win are dropped altogether.
+		goto dropafterack;
+
 		m_adj(m, -todrop);
 		tlen -= todrop;
 		thflags &= ~(TH_PUSH|TH_FIN);
@@ -2360,7 +3132,6 @@
 	 * The ACK was checked above.
 	 */
 	case TCPS_SYN_RECEIVED:
-
 		TCPSTAT_INC(tcps_connects);
 		soisconnected(so);
 		/* Do window scaling? */
@@ -2384,14 +3155,47 @@
 			    mtod(m, const char *), tp, th);
 			cc_conn_init(tp);
 			tcp_timer_activate(tp, TT_KEEP, TP_KEEPIDLE(tp));
+
+			/* MPTCP */
+			mp_wakeup |= tp_schedule_event(tp, SFE_CONNECTED);
+			if (to.to_flags & TOF_MPTCP) {
+				/* Have also become an MPTCP connection. Let the mp
+				 * control layer know. */
+				if(tp->t_sf_flags &
+				    (SFF_SENT_MPCAPABLE | SFF_FIRSTSUBFLOW)) {
+					tp->t_sf_state &= ~SFS_MP_CONNECTING;
+					tp->t_sf_flags &= ~SFF_INFINITEMAP;
+					tp->t_sf_state |= SFS_MP_ENABLED;
+					mp_wakeup |= tp_schedule_event(tp, SFE_MPESTABLISHED);
+				} else if(!(tp->t_sf_flags & SFF_FIRSTSUBFLOW)
+			          && (tp->t_sf_state & SFS_MP_CONNECTING)) {
+					tp->t_sf_state &= ~SFS_MP_CONNECTING;
+					tp->t_sf_state |= SFS_MP_ENABLED;
+					mp_debug(MPSESSION, 1, 0,
+						"%s:%d new mp connection for tp: %p: \n", __func__,
+						__LINE__, tp);
+					mp_debug(MPSESSION, 4, 0, "%s: set subflow as mp_active\n",
+					    __func__);
+				}
+			}
 		}
 		/*
 		 * If segment contains data or ACK, will call tcp_reass()
 		 * later; if not, do so now to pass queued data to user.
 		 */
-		if (tlen == 0 && (thflags & TH_FIN) == 0)
+		if (tlen == 0 && (thflags & TH_FIN) == 0) {
 			(void) tcp_reass(tp, (struct tcphdr *)0, 0,
 			    (struct mbuf *)0);
+			if (tp->t_segq_received) {
+//				printf("%s:%d received seg sqn %u\n", __func__, __LINE__,
+//				    th->th_seq);
+				if (m_ptr == NULL)
+					m_ptr = tp->t_segq_received;
+				else
+					mp_appendpkt(m_ptr, tp->t_segq_received);
+				tp->t_segq_received = NULL;
+			}
+		}
 		tp->snd_wl1 = th->th_seq - 1;
 		/* FALLTHROUGH */
 
@@ -2422,9 +3226,9 @@
 		hhook_run_tcp_est_in(tp, th, &to);
 
 		if (SEQ_LEQ(th->th_ack, tp->snd_una)) {
-			if (tlen == 0 && tiwin == tp->snd_wnd) {
-				/*
-				 * If this is the first time we've seen a
+			if (tlen == 0 && tiwin == tp->snd_wnd) {      //XXXNJW
+				/*                                                 //must
+				 * If this is the first time we've seen a          //change (??)
 				 * FIN from the remote, this is not a
 				 * duplicate and it needs to be processed
 				 * normally.  This happens during a
@@ -2435,6 +3239,15 @@
 					tp->t_dupacks = 0;
 					break;
 				}
+
+				/* XXXNJW: don't want to mark MPTCP signal-only
+				 * packets as dupacks. Find a nicer way to deal with
+				 * this. What about dup DFINs? */
+				if (to.to_mopts.mpo_flags &
+				    (MPOF_DATA_ACK | MPOF_DSN_MAP | MPOF_DATA_FIN)) {
+					tp->t_dupacks = 0;
+				}
+
 				TCPSTAT_INC(tcps_rcvdupack);
 				/*
 				 * If we have outstanding data (other than
@@ -2583,7 +3396,7 @@
 			} else
 				tp->t_dupacks = 0;
 			break;
-		}
+		} /* end dup-ack processing */
 
 		KASSERT(SEQ_GT(th->th_ack, tp->snd_una),
 		    ("%s: th_ack <= snd_una", __func__));
@@ -2706,9 +3519,13 @@
 			tp->snd_wnd -= acked;
 			ourfinisacked = 0;
 		}
+		dsmap_drop(&tp->t_send_maps.dsmap_list, th->th_ack,
+		    (ourfinisacked ? (int)sbavail(&so->so_snd) : acked));
+
 		/* NB: sowwakeup_locked() does an implicit unlock. */
 		sowwakeup_locked(so);
 		m_freem(mfree);
+
 		/* Detect una wraparound. */
 		if (!IN_RECOVERY(tp->t_flags) &&
 		    SEQ_GT(tp->snd_una, tp->snd_recover) &&
@@ -2720,6 +3537,10 @@
 			EXIT_RECOVERY(tp->t_flags);
 		}
 		tp->snd_una = th->th_ack;
+
+		if ((tp->t_sf_state & SFS_MP_ENABLED) == 0)
+			mp_wakeup |= MP_SCHEDINPUT;
+
 		if (tp->t_flags & TF_SACK_PERMIT) {
 			if (SEQ_GT(tp->snd_una, tp->snd_recover))
 				tp->snd_recover = tp->snd_una;
@@ -2747,6 +3568,8 @@
 				 * we should release the tp also, and use a
 				 * compressed state.
 				 */
+				mp_debug(MPSESSION, 1, 0,
+				    "%s: FIN is acked, enter FINWAIT_2 tp %p\n", __func__, tp);
 				if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
 					soisdisconnected(so);
 					tcp_timer_activate(tp, TT_2MSL,
@@ -2766,10 +3589,17 @@
 		 */
 		case TCPS_CLOSING:
 			if (ourfinisacked) {
+				if (tp->t_sf_state & SFS_MP_ENABLED)
+					mp_wakeup |= MP_SCHEDDETACH;
 				INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
 				tcp_twstart(tp);
 				INP_INFO_WUNLOCK(&V_tcbinfo);
 				m_freem(m);
+				if (mp_wakeup)
+					goto mp_input;
+				else
+					mpp_pcbrele_unlocked(mp->mp_mppcb);
+
 				return;
 			}
 			break;
@@ -2782,8 +3612,21 @@
 		 */
 		case TCPS_LAST_ACK:
 			if (ourfinisacked) {
+				printf("%s: subflow acked in LAST_ACK tp %p\n", __func__, tp);
 				INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
+				/* XXXNJW: to change.
+				 * For MP connections we track the count of subflows
+				 * and decrement as they close (in another thread).
+				 * Also clear the "MP_SCHEDINPUT" so we don't trigger
+				 * input processing in response to earlier segment
+				 * processing (as the inpcb/tcpcb won't exist soon
+				 * anyway) */
 				tp = tcp_close(tp);
+				mp_wakeup &= ~MP_SCHEDINPUT;
+				if (tp != NULL)
+					mp_wakeup |= MP_SCHEDCLOSE; /* Cases of disconnect only */
+				else
+					mp_wakeup |= MP_SCHEDDETACH;
 				goto drop;
 			}
 			break;
@@ -2880,7 +3723,7 @@
 
 	/*
 	 * Process the segment text, merging it into the TCP sequencing queue,
-	 * and arranging for acknowledgment of receipt if necessary.
+	 * and arranging for acknowledgement of receipt if necessary.
 	 * This process logically involves adjusting tp->rcv_wnd as data
 	 * is presented to the user (this happens in tcp_usrreq.c,
 	 * case PRU_RCVD).  If a FIN has already been received on this
@@ -2890,7 +3733,11 @@
 	    TCPS_HAVERCVDFIN(tp->t_state) == 0) {
 		tcp_seq save_start = th->th_seq;
 		m_adj(m, drop_hdrlen);	/* delayed header drop */
+		if (thflags & TH_FIN)
+			mp_debug(MPSESSION, 1, 0, "%s:%d: got FIN on tp %p\n", __func__,
+			    __LINE__, tp);
 		/*
+		 * XXXNJW: Need to adapt this comment
 		 * Insert segment which includes th into TCP reassembly queue
 		 * with control block tp.  Set thflags to whether reassembly now
 		 * includes a segment with FIN.  This handles the common case
@@ -2902,34 +3749,74 @@
 		 * immediately when segments are out of order (so
 		 * fast retransmit can work).
 		 */
+//		if (th->th_seq == tp->rcv_nxt && tp->t_segq == NULL &&
+//		    TCPS_HAVEESTABLISHED(tp->t_state)) {
+//			if (DELAY_ACK(tp, tlen))
+//				tp->t_flags |= TF_DELACK;
+//			else
+//				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;
+//		}
+
+        /* XXXNJW: Currently for MPTCP, always insert into the list.
+         * TCP reass checks for TCPS established and queues pre-established
+         * segments that have len.
+         */
 		if (th->th_seq == tp->rcv_nxt && tp->t_segq == NULL &&
-		    TCPS_HAVEESTABLISHED(tp->t_state)) {
-			if (DELAY_ACK(tp, tlen))
-				tp->t_flags |= TF_DELACK;
-			else
-				tp->t_flags |= TF_ACKNOW;
-			tp->rcv_nxt += tlen;
-			thflags = th->th_flags & TH_FIN;
+			TCPS_HAVEESTABLISHED(tp->t_state)) {
 			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);
+	        if (DELAY_ACK(tp, tlen))
+	            tp->t_flags |= TF_DELACK;
+		    else
+			    tp->t_flags |= TF_ACKNOW;
+		} else /* Out-of-order segment, ACK straight away */
+			tp->t_flags |= TF_ACKNOW;
+
+        /*
+		 * 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.
+		 */
+//		if (so->so_rcv.sb_state & SBS_CANTRCVMORE) {
+//			m_freem(m);
+//		} else {
+		    if (tlen)
+			    thflags = tcp_reass(tp, th, &tlen, m);
+		    else
+		    	thflags = th->th_flags & TH_FIN;
+//		}
+
+		if (tp->t_segq_received) {
+//			printf("%s: received seg sqn %u\n", __func__, th->th_seq);
+			if (m_ptr == NULL)
+			    m_ptr = tp->t_segq_received;
 			else
-				sbappendstream_locked(&so->so_rcv, m, 0);
-			/* 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;
+				mp_appendpkt(m_ptr, tp->t_segq_received);
+			tp->t_segq_received = NULL;
 		}
+
 		if (tlen > 0 && (tp->t_flags & TF_SACK_PERMIT))
 			tcp_update_sack_list(tp, save_start, save_start + tlen);
 #if 0
@@ -2996,13 +3883,19 @@
 		 * standard timers.
 		 */
 		case TCPS_FIN_WAIT_2:
+			if (tp->t_sf_state & SFS_MP_ENABLED)
+				mp_wakeup |= MP_SCHEDDETACH;
 			INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
 			KASSERT(ti_locked == TI_WLOCKED, ("%s: dodata "
 			    "TCP_FIN_WAIT_2 ti_locked: %d", __func__,
 			    ti_locked));
-
 			tcp_twstart(tp);
 			INP_INFO_WUNLOCK(&V_tcbinfo);
+			if (mp_wakeup)
+				goto mp_input;
+			else
+				mpp_pcbrele_unlocked(mp->mp_mppcb);
+
 			return;
 		}
 	}
@@ -3032,8 +3925,15 @@
 		tp->t_flags &= ~TF_DELACK;
 		tcp_timer_activate(tp, TT_DELACK, tcp_delacktime);
 	}
+
 	INP_WUNLOCK(tp->t_inpcb);
-	return;
+
+	if (m_ptr || mp_wakeup)
+		goto mp_input;
+	else
+		mpp_pcbrele_unlocked(mp->mp_mppcb);
+
+    return;
 
 dropafterack:
 	/*
@@ -3068,8 +3968,15 @@
 
 	tp->t_flags |= TF_ACKNOW;
 	(void) tcp_output(tp);
-	INP_WUNLOCK(tp->t_inpcb);
-	m_freem(m);
+
+    INP_WUNLOCK(tp->t_inpcb);
+    m_freem(m);
+
+	if (m_ptr || mp_wakeup)
+		goto mp_input;
+	else
+		mpp_pcbrele_unlocked(mp->mp_mppcb);
+
 	return;
 
 dropwithreset:
@@ -3080,8 +3987,15 @@
 	if (tp != NULL) {
 		tcp_dropwithreset(m, th, tp, tlen, rstreason);
 		INP_WUNLOCK(tp->t_inpcb);
-	} else
+		/* Further processing at mp-layer */
+		if (m_ptr || mp_wakeup)
+			goto mp_input;
+		else
+			mpp_pcbrele_unlocked(mp->mp_mppcb);
+	} else {
 		tcp_dropwithreset(m, th, NULL, tlen, rstreason);
+		mpp_pcbrele_unlocked(mp->mp_mppcb);
+	}
 	return;
 
 drop:
@@ -3105,6 +4019,46 @@
 	if (tp != NULL)
 		INP_WUNLOCK(tp->t_inpcb);
 	m_freem(m);
+
+	/* Further processing at mp-layer */
+	/* Might we want to wakeup even when a tp has been de-alloced? */
+    if (m_ptr || mp_wakeup)
+        goto mp_input;
+    else
+    	mpp_pcbrele_unlocked(mp->mp_mppcb);
+
+    return;
+
+    /* XXXNJW: Kind of a stop-gap way to trigger appropriate tasks
+     * at the MP-layer. To re-think. (Perhaps an mptcp-specific
+     * upcall?) */
+mp_input:
+	MPP_LOCK(mp->mp_mppcb);
+
+	/* If this was the last reference on the mpp, then the session
+	 * has ended and we just return now. (all the data structures
+	 * will have been freed by now). */
+	if (mpp_pcbrele(mp->mp_mppcb)) {
+		printf("%s: freed mpp %p mp %p\n", __func__, mp->mp_mppcb, mp);
+		return;
+	}
+
+	if (m_ptr) {
+	    mp_mbuf_enqueue(mp, m_ptr);
+        mp_wakeup |= MP_SCHEDINPUT;
+	}
+	if ((mp_wakeup & (MP_SCHEDINPUT|MP_SCHEDEVENT|MP_SCHEDJOIN)) != 0)
+	    mp_schedule_tasks(mp, mp_wakeup);
+
+	/* XXNJW: Might have some tasks that can be performed right now.
+	 * this is just a cheap workaround until better monitoring of
+	 * the subflow socket is implemented (say to pick up on
+	 * soisdisconnected and that sort of thing) */
+	if ((mp_wakeup & (MP_SCHEDDETACH | MP_SCHEDCLOSE)) != 0)
+		if (mp_do_task_now(mp, mp_wakeup))
+			return;
+
+	MPP_UNLOCK(mp->mp_mppcb);
 }
 
 /*
@@ -3179,7 +4133,9 @@
 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];
@@ -3224,6 +4180,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
@@ -3379,6 +4342,14 @@
 	tp->t_softerror = 0;
 }
 
+static int
+tp_schedule_event(struct tcpcb *tp, int event)
+{
+	tp->t_event_flags |= event;
+    tp->t_event_pending = 1;
+	return MP_SCHEDEVENT;
+}
+
 /*
  * Determine a reasonable value for maxseg size.
  * If the route is known, check route for mtu.
diff -r 1d1c4c997b66 sys/netinet/tcp_output.c
--- a/sys/netinet/tcp_output.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_output.c	Sun Aug 30 14:27:42 2015 +1000
@@ -37,10 +37,13 @@
 #include "opt_ipsec.h"
 #include "opt_tcpdebug.h"
 
+#include <sys/endian.h>
+
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/domain.h>
 #include <sys/hhook.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/lock.h>
 #include <sys/mbuf.h>
@@ -50,6 +53,7 @@
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 
 #include <net/if.h>
 #include <net/route.h>
@@ -80,7 +84,9 @@
 #ifdef TCP_OFFLOAD
 #include <netinet/tcp_offload.h>
 #endif
-
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_var.h>
 #ifdef IPSEC
 #include <netipsec/ipsec.h>
 #endif /*IPSEC*/
@@ -94,7 +100,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_INT(_net_inet_tcp, OID_AUTO, tso, CTLFLAG_VNET | CTLFLAG_RW,
 	&VNET_NAME(tcp_do_tso), 0,
@@ -128,6 +134,9 @@
 			    long len, int tso);
 static void inline	cc_after_idle(struct tcpcb *tp);
 
+static void inline 	mp_addoptions(struct tcpopt *to,
+				u_int *optlen_ptr, u_char *optp);
+
 /*
  * Wrapper for the TCP established output helper hook.
  */
@@ -161,6 +170,369 @@
 		CC_ALGO(tp)->after_idle(tp->ccv);
 }
 
+static void inline
+mp_addoptions(struct tcpopt *to, u_int *optlen_ptr, u_char *optp) {
+	struct mp_capable mcap;
+
+	uint64_t mask = 0;
+	for (mask = 1; mask < MPOF_MAXOPT; mask <<= 1) {
+		if ((to->to_mopts.mpo_flags & mask) != mask)
+			continue;
+		if (*optlen_ptr == TCP_MAXOLEN)
+			break;
+		switch (to->to_mopts.mpo_flags & mask)
+		{
+		case MPOF_CAPABLE_SYN:
+		{
+			/*
+			 * Note that keys are stored in network order, so
+			 * do no need to call htobe64 before bcopy
+			 */
+			mcap.kind = TCPOPT_MPTCP;
+			mcap.ver_sub = MPTCP_SUBTYPE_MP_CAPABLE;
+
+			uint8_t cap_flags = 0;
+			cap_flags |= USE_SHA1; // hard coded in sha1
+			cap_flags &= ~USE_CSUM; // don't support csumming yet
+
+			if (to->to_mopts.mpo_flags & MPOF_USE_CSUM) {
+				cap_flags |= USE_CSUM;
+			}
+			/* sending a SYN */
+
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_CAPABLE_SYN)
+				break;
+
+			mcap.length = MPTCP_SUBLEN_MP_CAPABLE_SYN;
+
+			/* SYN (or SYNACK), just include the local key */
+			u_int64_t key = to->to_mopts.local_key;
+
+			/* MP_CAPABLE type/flags */
+			*optlen_ptr += MPTCP_SUBLEN_MP_CAPABLE_SYN;
+			*optp++ = mcap.kind;
+			*optp++ = mcap.length;
+			*optp++ = 0; // version and subtype are 0
+			*optp++ = cap_flags;
+
+			/* copy in the key s*/
+			bcopy((u_char *)&key, optp, sizeof(key));
+			optp += sizeof(key);
+
+			/*
+			 * Indicate to caller that the capable option has been
+			 * copied into the outgoing TCP header.
+			 */
+			to->to_mopts.mpo_flags &= ~MPOF_CAPABLE_SYN;
+			break;
+		}
+		case MPOF_CAPABLE_ACK:
+		{
+			uint8_t cap_flags = 0;
+			/* ACK of 3-way handshake */
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_CAPABLE_ACK)
+				break;
+
+			u_int64_t local_key = to->to_mopts.local_key;
+			u_int64_t remote_key = to->to_mopts.remote_key;
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_CAPABLE_ACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_CAPABLE_ACK;
+			*optp++ = 0;	// just manually setting
+			*optp++ = cap_flags;
+
+			/* and copy in the local key */
+			bcopy((u_char *)&local_key, optp, sizeof(local_key));
+			optp += sizeof(local_key);
+
+			/* echo the remote key */
+			bcopy((u_char *)&remote_key, optp, sizeof(remote_key));
+			optp += sizeof(remote_key);
+
+			break;
+		}
+		case MPOF_MP_JOIN: /* Sending a MP_JOIN SYN Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_SYN)
+				break;
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+
+			/*
+			 * XXXNJW: just assuming for now that SYNs are sent as subflows
+			 * are added
+			 */
+			uint8_t addrID = to->to_mopts.addr_id;
+			uint32_t remote_token = htonl(to->to_mopts.rcv_token);
+
+			subtype = (subtype << 4);
+
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = (MPTCP_SUBTYPE_MP_JOIN << 4);	// XXXNJW manually setting
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_JOIN_SYN;
+			*optp++ = addrID;
+
+			printf("%s: copied rcv's token %u\n", __func__,                     //XXXNJW
+			    to->to_mopts.rcv_token);
+			printf("%s: rcv token htonl %u\n", __func__,
+					htonl(to->to_mopts.rcv_token));
+
+			bcopy((u_char *)&remote_token,
+					optp, sizeof(remote_token));
+			optp += sizeof(remote_token);
+
+			bcopy((u_char *)&to->to_mopts.snd_rnd,
+					optp, sizeof(to->to_mopts.snd_rnd));
+			optp += sizeof(to->to_mopts.snd_rnd);
+			break;
+		}
+		case MPOF_JOIN_SYN: /* Sending a MP_JOIN SYNACK Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_SYNACK)
+				break;
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+			subtype = (subtype << 4);
+
+			*optlen_ptr += MPTCP_SUBLEN_MP_JOIN_SYNACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_SYNACK;
+			*optp++ = subtype;
+			*optp++ = (uint8_t) to->to_mopts.addr_id;
+
+			bcopy((u_char *)&to->to_mopts.truncated_MAC,
+			    optp, sizeof(to->to_mopts.truncated_MAC));
+			optp += sizeof(to->to_mopts.truncated_MAC);
+
+			bcopy((u_char *)&to->to_mopts.snd_rnd,
+			    optp, sizeof(to->to_mopts.snd_rnd));
+			optp += sizeof(to->to_mopts.snd_rnd);
+			break;
+		}
+		case MPOF_JOIN_SYNACK: /* Sending a MP_JOIN ACK Option */
+		{
+			if (TCP_MAXOLEN - *optlen_ptr < MPTCP_SUBLEN_MP_JOIN_ACK) {
+				break;
+			}
+
+			uint8_t subtype = MPTCP_SUBTYPE_MP_JOIN;
+			subtype = (subtype << 4);
+
+			*optlen_ptr += (uint8_t) MPTCP_SUBLEN_MP_JOIN_ACK;
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = MPTCP_SUBLEN_MP_JOIN_ACK;
+			*optp++ = subtype;
+			/* the last 12-bits after subtype are reserved (zeroed) */
+			*optp++ = 0;
+
+			/* Copy full HMAC-A into option */
+			bcopy(to->to_mopts.snd_mac, optp, 20);
+			optp += 20;
+			break;
+		}
+		case MPOF_DSS:
+		{
+			uint8_t subtype = MPTCP_SUBTYPE_DSS;
+			uint8_t dss_opts = 0;
+			uint8_t length = 4; /* the initial 4 bytes of option */
+
+			/* Set flags for DSS */
+			if (to->to_mopts.mpo_flags & MPOF_DATA_FIN) {
+				printf("%s: setting Data-fin\n", __func__);
+				dss_opts |= FIN_PRESENT;
+			}
+
+			if (to->to_mopts.mpo_flags & MPOF_DATA_ACK) {
+				dss_opts |= ACK_PRESENT;
+				length += 4;
+
+				if (to->to_mopts.mpo_flags & MPOF_ACK64) {
+					dss_opts |= ACK_64_PRESENT;
+					length += 4;
+				}
+			}
+
+			if (to->to_mopts.mpo_flags & MPOF_DSN_MAP) {
+				dss_opts |= MAP_PRESENT;
+				length += 4;
+
+				length += 4;	/* subflow sequence num */
+				length += 2;	/* data length */
+
+				if(to->to_mopts.mpo_flags & MPOF_USE_CSUM)
+					length += 2;
+
+				if (to->to_mopts.mpo_flags & MPOF_DSN64) {
+					length += 4;
+					dss_opts |= DSN_64;
+				}
+			}
+
+			if (TCP_MAXOLEN - *optlen_ptr < length)
+				break;
+
+			*optlen_ptr += length;
+
+			// pad it out to divide by 4 to prevent EOL being inserted
+			// before our option
+			// XXXNJW kind of a hack, to fix
+			if (*optlen_ptr % 4) {
+				*optlen_ptr += TCPOLEN_NOP;
+				*optp++ = TCPOPT_NOP;
+				*optlen_ptr += TCPOLEN_NOP;
+				*optp++ = TCPOPT_NOP;
+			}
+
+			/* option kind and length fields */
+			*optp++ = TCPOPT_MPTCP;
+			*optp++ = length;
+
+			/* subtype and flags */
+			subtype = (subtype << 4);
+			bcopy((u_char *)&subtype, optp, sizeof(uint8_t));
+			optp += sizeof(uint8_t);
+//			to->to_mopts.dss_opts_f_p = (uint8_t*) optp;
+			bcopy((u_char *)&dss_opts, optp, sizeof(uint8_t));
+			optp += sizeof(uint8_t);
+
+			/* copy the Data ACK */
+			if (to->to_mopts.mpo_flags & MPOF_DATA_ACK) {
+				if (to->to_mopts.mpo_flags & MPOF_ACK64) {
+					u_int64_t data_ack_num = to->to_mopts.data_ack_num;
+					data_ack_num = htobe64(data_ack_num);
+					bcopy((u_char *)&data_ack_num, optp, sizeof(data_ack_num));
+					optp += sizeof(data_ack_num);
+				} else {
+					u_int32_t data_ack_num =
+					    (uint32_t) to->to_mopts.data_ack_num;
+					data_ack_num = htonl(data_ack_num);
+					bcopy((u_char *)&data_ack_num, optp, sizeof(data_ack_num));
+					optp += sizeof(data_ack_num);
+				}
+			}
+
+			/* DSN contents */
+			if (to->to_mopts.mpo_flags & MPOF_DSN_MAP) {
+				uint32_t data_seq_num = (uint32_t) to->to_mopts.data_seq_num;
+				/* DSN and SSN */
+				if (to->to_mopts.mpo_flags & MPOF_DSN64) {
+					to->to_mopts.data_seq_num =
+							htobe64(to->to_mopts.data_seq_num);
+					bcopy((u_char *)&to->to_mopts.data_seq_num,
+							optp, sizeof(to->to_mopts.data_seq_num));
+					optp += sizeof(to->to_mopts.data_seq_num);
+				} else {
+					data_seq_num = htonl(data_seq_num);
+					bcopy((u_char *)&data_seq_num, optp, sizeof(data_seq_num));
+					optp += sizeof(data_seq_num);
+				}
+
+				to->to_mopts.sub_seq_num = htonl(to->to_mopts.sub_seq_num);
+				bcopy((u_char *)&to->to_mopts.sub_seq_num, optp,
+					sizeof(to->to_mopts.sub_seq_num));
+				optp += sizeof(to->to_mopts.sub_seq_num);
+
+				/* Length */
+				/* set length after we know the size, for a map-per-packet
+				 * scenario */
+				to->to_mopts.dss_data_len_p = optp;
+//				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 all 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;
+			to->to_mopts.mpo_flags &= ~MPOF_DATA_FIN;
+			break;
+		}
+		case MPOF_ADD_ADDR:
+		{
+			struct mp_add address;
+			int i, addr_id, addr_len = 0;
+			void * addr = NULL;
+
+			address.length = address.sub_ipver = 0;
+
+			/* Loop through the available addresses. */
+			for (i = 0; i < to->to_mopts.address_count; i++) {
+				/* continue if there are addresses to advertise */
+				if (!(to->to_mopts.add_addr_mask & (1 << i)))
+					continue;
+
+				switch (mp_usable_addresses[i].ss_family) {
+					case AF_INET:
+						addr = &((struct sockaddr_in *)
+							&mp_usable_addresses[i])->sin_addr.s_addr;
+						address.length = MPTCP_SUBLEN_ADD_ADDRV4;
+						address.sub_ipver = 4;
+						addr_len = 4;
+						break;
+					case AF_INET6:
+						addr = &((struct sockaddr_in6 *)
+							&mp_usable_addresses[i])->sin6_addr.__u6_addr;
+						address.length = MPTCP_SUBLEN_ADD_ADDRV6;
+						address.sub_ipver = 6;
+						addr_len = 16;
+						break;
+				}
+
+				if (TCP_MAXOLEN - *optlen_ptr < address.length)
+					break;
+
+				/* mark address as advertised. Need to pass this back to the
+				 * mpcb */
+				to->to_mopts.add_addr_mask &= ~(1 << i);
+
+				*optlen_ptr += address.length;
+				*optp++ = TCPOPT_MPTCP;
+				*optp++ = address.length;
+
+				/* subtype and IPVer */
+				uint8_t subtype = (MPTCP_SUBTYPE_ADD_ADDR << 4);
+				subtype |= address.sub_ipver;
+				bcopy((u_char *)&subtype, optp, sizeof(uint8_t));
+				optp ++;
+
+				/* Minus 1 as we never send add_addr for the master subflow */
+				addr_id = i - 1;
+
+				/* Address ID */
+				bcopy((u_char *)&addr_id, optp, sizeof(uint8_t));
+				optp ++;
+
+				bcopy((u_char *)addr, optp, addr_len);
+				optp += addr_len;
+			}
+			break;
+		}
+		case MPOF_REMOVE_ADDR:
+			break;
+		case MPOF_MP_FAIL:
+			break;
+		case MPOF_FASTCLOSE:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
 /*
  * Tcp output routine: figure out what should be sent and send it.
  */
@@ -168,8 +540,10 @@
 tcp_output(struct tcpcb *tp)
 {
 	struct socket *so = tp->t_inpcb->inp_socket;
+	struct mpcb *mp = tp->t_mpcb;
 	long len, recwin, sendwin;
-	int off, flags, error = 0;	/* Keep compiler happy */
+	int off = 0, flags, map_offset = 0, error = 0;	/* Keep compiler happy */
+	struct ds_map *snd_dsmap;
 	struct mbuf *m;
 	struct ip *ip = NULL;
 	struct ipovly *ipov = NULL;
@@ -195,6 +569,7 @@
 #endif
 
 	INP_WLOCK_ASSERT(tp->t_inpcb);
+	KASSERT(mp != NULL, ("%s: mp NULL", __func__));
 
 #ifdef TCP_OFFLOAD
 	if (tp->t_flags & TF_TOE)
@@ -217,6 +592,7 @@
 			idle = 0;
 		}
 	}
+
 again:
 	/*
 	 * If we've recently taken a timeout, snd_max will be greater than
@@ -348,13 +724,11 @@
 	 */
 	if (sack_rxmit == 0) {
 		if (sack_bytes_rxmt == 0)
-			len = ((long)ulmin(sbavail(&so->so_snd), sendwin) -
-			    off);
+			len = ((long)ulmin(sbavail(&so->so_snd), sendwin) - off);
 		else {
 			long cwin;
 
-                        /*
-			 * We are inside of a SACK recovery episode and are
+			/* We are inside of a SACK recovery episode and are
 			 * sending new data, having retransmitted all the
 			 * data possible in the scoreboard.
 			 */
@@ -513,6 +887,13 @@
 
 	recwin = sbspace(&so->so_rcv);
 
+//	if (recwin > (long)TCP_MAXWIN) {
+//		printf("%s: recwin = sbspace = %ld scale %d \n", __func__, recwin,
+//		tp->rcv_scale);
+////		kdb_break();
+//	}
+
+
 	/*
 	 * Sender silly window avoidance.   We transmit under the following
 	 * conditions when len is non-zero:
@@ -710,6 +1091,7 @@
 	 */
 	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;
@@ -721,6 +1103,192 @@
 			to.to_wscale = tp->request_r_scale;
 			to.to_flags |= TOF_SCALE;
 		}
+        /* MPTCP options */
+		if (V_tcp_do_mptcp) {
+			if (flags & TH_SYN) {
+				/* Multipath announce capable, for SYN and SYN/ACK */
+				if (tp->t_sf_flags & SFF_SEND_MPCAPABLE) {
+					tp->t_sf_flags &= ~SFF_SEND_MPCAPABLE;
+					tp->t_sf_flags |= SFF_SENT_MPCAPABLE;
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_CAPABLE_SYN;
+					mp_process_local_key(&tp->t_mp_conn,
+					    mp_generate_local_key());
+					to.to_mopts.local_key = tp->t_mp_conn.local_key;
+				} else {
+					/* A SYN on a subflow that isn't the first, use
+					 * MP_JOIN. In this case an MP session has already
+					 * been established. */
+					tp->t_sf_state |= SFS_MP_CONNECTING;
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_MP_JOIN;
+					to.to_mopts.rcv_token =
+							tp->t_mp_conn.remote_token;
+					tp->t_mp_conn.local_rand = to.to_mopts.snd_rnd =
+					    arc4random();
+					to.to_mopts.addr_id = tp->t_addrid - 1;
+				}
+			}
+
+			/* received a SYNACK with MP_CAPABLE */
+			if (tp->t_sf_flags & SFF_GOT_SYNACK) {
+				tp->t_sf_flags &= ~SFF_GOT_SYNACK;
+				/* got a synack after sending capable */
+
+				if (tp->t_sf_flags & SFF_SENT_MPCAPABLE) {
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_CAPABLE_ACK;
+					to.to_mopts.local_key = tp->t_mp_conn.local_key;
+					to.to_mopts.remote_key = tp->t_mp_conn.remote_key;
+				}
+			}
+
+			/* Received a SYNACK with MP_JOIN */
+			if (tp->t_sf_flags & SFF_GOT_JOIN_SYNACK) {
+				/* XXXNJW: should clear this only when we know that the
+				 * option has been copied into a packet.
+				 */
+				tp->t_sf_flags &= ~SFF_GOT_JOIN_SYNACK;
+
+				/* XXXNJW: Should set this state only when we know the
+				 * option has been put into packet */
+				tp->t_sf_state |= SFS_MP_CONNECTING;
+
+				/* Must send full HMAC-A as third part of MP_JOIN
+				 * handshake */
+				to.to_mopts.snd_mac = tp->t_mp_conn.hmac_local;
+
+				/* Add an MP_JOIN_ACK to the outgoing packet */
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_JOIN_SYNACK;
+			}
+
+		} else {
+			/* Need to init these even if not doing multipath, as the
+			 * accounting requires it. */
+//			MP_INNER_LOCK(tp->t_mpcb);
+//			mp_sendseqinit(tp->t_mpcb, tp);
+//			MP_INNER_UNLOCK(tp->t_mpcb);
+		}
+
+		/* Do these options only once mptcp is established. */
+		if (tp->t_sf_state & SFS_MP_ENABLED) {
+			/* XXXNJW:
+			 * A weird thing about this is that we only trigger
+			 * the advertisement of addresses in mp_do_output,
+			 * which is called under the MPP_LOCK. Hence for now
+			 * just pulling the address info directly out of the
+			 * mpcb. We should only ever enter into here while
+			 * under an MPP lock.
+			 *
+			 * Should replace this
+			 */
+			if (tp->t_sf_flags & SFF_SEND_ADD_ADDR) {
+				MPP_LOCK_ASSERT(tp->t_mpcb->mp_mppcb);
+				tp->t_sf_flags &= ~SFF_SEND_ADD_ADDR;
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= MPOF_ADD_ADDR;
+				to.to_mopts.add_addr_mask = tp->t_mpcb->mp_advaddr_mask;
+				to.to_mopts.address_count = tp->t_mpcb->mp_conn_address_count;
+				to.to_mopts.addr_id = tp->t_mpcb->subflow_cnt - 1;
+			}
+
+			/*
+			 * Need to attach a DATA-ACK to the outgoing packet
+			 * MPO_DATA_ACK is usually 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_sf_flags & SFF_NEED_DACK) {
+				tp->t_sf_flags &= ~SFF_NEED_DACK;
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= (MPOF_DATA_ACK | MPOF_DSS);
+				// should check against session settings
+				//to.to_mopts.mpo_flags |= MPOF_DSS;
+				//to.to_mopts.mpo_flags |= MPOF_ACK64;
+				to.to_mopts.data_ack_num =
+				    (uint32_t) tp->t_mp_conn.ds_ack_num;
+//				DACK value to be set in tcpcb by mp_output.
+			}
+
+			/* Connection is closing */
+			if (tp->t_sf_flags & SFF_NEED_DFIN) {
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |= (MPOF_DATA_FIN | MPOF_DSS);
+
+				/* A DFIN with no payload requires:
+				 * - SSN == 0
+				 * - Data-len == 1
+				 * - DSN == The next DSN to send
+				 */
+				if (len == 0) {
+					to.to_mopts.mpo_flags |= MPOF_DSN_MAP;
+					to.to_mopts.mpo_flags |= MPOF_DSS;
+				    to.to_mopts.sub_seq_num = 0;
+				    to.to_mopts.dss_data_len = 1;
+
+				    /* mp_do_output pushes ds_snd_nxt by one when dfin
+				     * is set. There is no map with a zero-len segment
+				     * (typically dsn is based on map offset) so must
+				     * subtract '1' here to make sure we send the next
+				     * expected data-level byte.
+				     * XXXNJW: to fix */
+					to.to_mopts.data_seq_num =
+					    (uint32_t) tp->t_mp_conn.ds_snd_nxt - 1;
+
+					// change to use ds_snd_nxt specified by mp-layer
+					// (in e.g. tp->t_ds_snd_nxt)
+
+					snd_dsmap = NULL;
+				}
+			}
+
+			/* Find the ds map for this tcp sqn */
+		    snd_dsmap = (len) ? mp_find_dsmap(tp, tp->snd_nxt): NULL;
+
+			if (len)
+			    KASSERT(snd_dsmap != NULL,
+			        ("%s: mp enabled, have len but no map\n", __func__));
+
+			if (snd_dsmap != NULL) {
+     			map_offset = tp->snd_nxt - snd_dsmap->sf_seq_start;
+
+				/* Currently puts a DSN map on each packet. */
+				to.to_mopts.mpo_flags |= MPOF_DSN_MAP;
+
+				/* XXXNJW: Need to put in options for either mult-packet maps or
+				 * per-packet maps.
+				 *
+				 * Need to build a DSS MAP for the outgoing packet. Note that the
+				 * mapping here does not always map directly to the ds_map. In the
+				 * following case we are creating smaller maps for each segment
+				 * from the ds_map */
+				if (to.to_mopts.mpo_flags & MPOF_DSN_MAP) {
+					/* XXXNJW: implement checks for csum and dsn64 */
+					//to.to_mopts.mpo_flags |= MPOF_USE_CSUM;
+					//to.to_mopts.mpo_flags |= MPOF_DSN64;
+
+					to.to_flags |= TOF_MPTCP;
+					to.to_mopts.mpo_flags |= MPOF_DSS;
+
+					/* This will create a map for each segment, of length 'len' */
+					to.to_mopts.sub_seq_num =
+						(snd_dsmap->sf_seq_start + map_offset - tp->iss);
+					to.to_mopts.data_seq_num =
+						(uint32_t) snd_dsmap->ds_map_start + map_offset;
+					//to.to_mopts.dss_data_len = len;
+					if (to.to_mopts.data_seq_num == 0) {
+						printf("%s: dsn is 0 len %ld tsqn %u\n", __func__,
+						    len, tp->snd_nxt);
+					}
+
+				}
+			}
+		}
+
 		/* Timestamps. */
 		if ((tp->t_flags & TF_RCVD_TSTMP) ||
 		    ((flags & TH_SYN) && (tp->t_flags & TF_REQ_TSTMP))) {
@@ -750,10 +1318,22 @@
 			to.to_flags |= TOF_SIGNATURE;
 #endif /* TCP_SIGNATURE */
 
+		/* Clear MPTCP options from RST packets */
+		if (flags & TH_RST)
+		    to.to_flags &= ~TOF_MPTCP;
+
 		/* Processing the options. */
 		hdrlen += optlen = tcp_addoptions(&to, opt);
 	}
 
+	/* XXXNJW to remove */
+	/* update the address masks */
+	if (to.to_mopts.mpo_flags & MPOF_ADD_ADDR) {
+	    to.to_mopts.mpo_flags &= ~MPOF_ADD_ADDR;
+	    MPP_LOCK_ASSERT(tp->t_mpcb->mp_mppcb);
+	    tp->t_mpcb->mp_advaddr_mask = to.to_mopts.add_addr_mask;
+	}
+
 #ifdef INET6
 	if (isipv6)
 		ipoptlen = ip6_optlen(tp->t_inpcb);
@@ -906,6 +1486,54 @@
 	KASSERT(len + hdrlen + ipoptlen <= IP_MAXPACKET,
 	    ("%s: len > IP_MAXPACKET", __func__));
 
+	/* XXXNJW: This is a temporary hack way to adjust ds map length
+	 * after the options have been added. This is only applicable for
+	 * map-per-packet cases, where the length of the map is the length
+	 * of the transmitted segment.
+	 *
+	 * Should probably just have longer maps and include the same
+	 * map details multiple times until we've sent all the data covered
+	 * by that map?
+	 *
+	 * also need a better solution to segments overlapping map boundaries
+	 * than clamping down to map_remain
+	 */
+//	if (len && (tp->t_sf_state & SFS_MP_ENABLED)) {
+//	    long unsent = snd_dsmap->ds_map_len - map_offset;
+//	    len = min((uint32_t)len, unsent);
+//	    to.to_mopts.dss_data_len = htons((uint16_t)len);
+//	}
+//
+//	/* Now copy the length into the length field od the DSS option */
+//	if ((tp->t_sf_state & SFS_MP_ENABLED) &&
+//	    (to.to_mopts.mpo_flags & MPOF_DSS)) {
+//		if(to.to_mopts.dss_data_len)
+//			bcopy((u_char *)&to.to_mopts.dss_data_len,
+//				to.to_mopts.dss_data_len_p, sizeof(to.to_mopts.dss_data_len));
+//	}
+
+	if ((tp->t_sf_state & SFS_MP_ENABLED) && (to.to_flags & TOF_MPTCP)) {
+		long unsent = 0;
+		if (len) {
+	        unsent = snd_dsmap->ds_map_len - map_offset;
+			if(unsent == 0) {
+				printf("%s: len %ld unsent %ld, ds_map_len %d\n",
+				    __func__, len, unsent, (uint32_t) snd_dsmap->ds_map_len);
+				kdb_break();
+			}
+	        len = min((uint32_t)len, unsent);
+	        to.to_mopts.dss_data_len = htons((uint16_t)len);
+	        bcopy((u_char *)&to.to_mopts.dss_data_len,
+				to.to_mopts.dss_data_len_p, sizeof(to.to_mopts.dss_data_len));
+		} else if (tp->t_sf_flags & SFF_NEED_DFIN) {
+		    to.to_mopts.dss_data_len =
+		        htons((uint16_t)to.to_mopts.dss_data_len);
+		    bcopy((u_char *)&to.to_mopts.dss_data_len,
+				to.to_mopts.dss_data_len_p, sizeof(to.to_mopts.dss_data_len));
+		}
+		tp->t_sf_flags &= ~SFF_NEED_DFIN; /* XXXNJW: a workaround */
+	}
+
 /*#ifdef DIAGNOSTIC*/
 #ifdef INET6
 	if (max_linkhdr + hdrlen > MCLBYTES)
@@ -1651,6 +2279,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 1d1c4c997b66 sys/netinet/tcp_reass.c
--- a/sys/netinet/tcp_reass.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_reass.c	Sun Aug 30 14:27:42 2015 +1000
@@ -37,6 +37,7 @@
 #include "opt_tcpdebug.h"
 
 #include <sys/param.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
@@ -69,6 +70,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>
 
@@ -97,6 +100,7 @@
 {
 	struct socket *so = tp->t_inpcb->inp_socket;
 	struct mbuf *mq, *mp;
+	struct mbuf *last_seg;
 	int flags, wakeup;
 
 	INP_WLOCK_ASSERT(tp->t_inpcb);
@@ -214,42 +218,71 @@
 		mq = nq;
 	}
 
+	/* Insert the new segment queue entry into place. */
+//	if (mp) {
+//		if (M_TCPHDR(mp)->th_seq + mp->m_pkthdr.len == th->th_seq)
+//			m_catpkt(mp, m);
+//		else {
+//			m->m_nextpkt = mp->m_nextpkt;
+//			mp->m_nextpkt = m;
+//			m->m_pkthdr.pkt_tcphdr = th;
+//		}
+//	} else {
+//		mq = tp->t_segq;
+//		tp->t_segq = m;
+//		if (mq && th->th_seq + *tlenp == M_TCPHDR(mq)->th_seq) {
+//			m->m_nextpkt = mq->m_nextpkt;
+//			mq->m_nextpkt = NULL;
+//			m_catpkt(m, mq);
+//		} else
+//			m->m_nextpkt = mq;
+//		m->m_pkthdr.pkt_tcphdr = th;
+//	}
+
 	/*
-	 * Insert the new segment queue entry into place.  Try to collapse
-	 * mbuf chains if segments are adjacent.
+	 * Insert the new segment queue entry into place.
+     *
+     * XXXNJW: Segments that are adjacent at TCP-level might not represent
+     * contiguous bytes at the data-level, thus don't collapse the segments
+     * in MPTCP, as we need to retain the mbuf header and DSN tag for later
+     * data-level reassembly
 	 */
 	if (mp) {
-		if (M_TCPHDR(mp)->th_seq + mp->m_pkthdr.len == th->th_seq)
-			m_catpkt(mp, m);
-		else {
-			m->m_nextpkt = mp->m_nextpkt;
-			mp->m_nextpkt = m;
-			m->m_pkthdr.pkt_tcphdr = th;
-		}
+		m->m_nextpkt = mp->m_nextpkt;
+		mp->m_nextpkt = m;
 	} else {
 		mq = tp->t_segq;
 		tp->t_segq = m;
-		if (mq && th->th_seq + *tlenp == M_TCPHDR(mq)->th_seq) {
-			m->m_nextpkt = mq->m_nextpkt;
-			mq->m_nextpkt = NULL;
-			m_catpkt(m, mq);
-		} else
-			m->m_nextpkt = mq;
+		m->m_nextpkt = mq;
 		m->m_pkthdr.pkt_tcphdr = th;
 	}
 	tp->t_segqlen += *tlenp;
 
 present:
+
+//    if (th) {
+//		if (th->th_seq != tp->rcv_nxt)
+//			printf("%s: present - rcv_nxt %u tseq %u tp %p\n", __func__,
+//				tp->rcv_nxt, th->th_seq, tp);
+//    }
+
+//
+//    if(tp->t_segq)
+//    	printf("%s: first seg %u\n", __func__,
+//    	    (uint32_t) M_TCPHDR(tp->t_segq)->th_seq);
+
 	/*
-	 * Present data to user, advancing rcv_nxt through
+	 * Adjust accounting advancing rcv_nxt through
 	 * completed sequence space.
 	 */
 	if (!TCPS_HAVEESTABLISHED(tp->t_state))
 		return (0);
 
+	KASSERT(tp->t_segq_received == NULL,
+	    ("%s: t_segq_received not NULL\n", __func__));
+
+    wakeup = 0;
 	flags = 0;
-	wakeup = 0;
-	SOCKBUF_LOCK(&so->so_rcv);
 	while ((mq = tp->t_segq) != NULL &&
 	    M_TCPHDR(mq)->th_seq == tp->rcv_nxt) {
 		tp->t_segq = mq->m_nextpkt;
@@ -262,14 +295,47 @@
 			m_freem(mq);
 		else {
 			mq->m_nextpkt = NULL;
-			sbappendstream_locked(&so->so_rcv, mq, 0);
-			wakeup = 1;
+			/* Now queue up the segment in the received list. On return
+			 * t_segq_received is assigned to a local-scope pointer and
+			 * set to NULL. The pointer is enqueued in mp_input_segq */
+			if (tp->t_segq_received) {
+				last_seg->m_nextpkt = mq;
+				last_seg = mq;
+			} else {
+				tp->t_segq_received = mq;
+				last_seg = tp->t_segq_received;
+			}
 		}
 	}
-	ND6_HINT(tp);
-	if (wakeup)
-		sorwakeup_locked(so);
-	else
-		SOCKBUF_UNLOCK(&so->so_rcv);
+
 	return (flags);
 }
+
+
+
+/* cleanup segments that have been bypassed (e.g. due to rexmit
+ * The mp_reass code might actually do something like this for us,
+ * since insertion of the segment into the data-level list will trim/drop
+ * segments.
+ *
+ * NB: very late segments will still need to be handled in mp_reass (i.e. if
+ * they are less than ds_rcv_nxt)
+ *
+ */
+
+///* Get mbuf tag with DSN */
+//mtag =  m_tag_locate(mq, PACKET_COOKIE_MPTCP, PACKET_TAG_DSN, NULL);
+//KASSERT(mtag != NULL, ("%s segment %u missing an mbuf tag\n",
+//    __func__, th->th_seq));
+//m_dsn = ((struct dsn_tag *)mtag)->dsn;
+//
+///* Has this segment been bypassed at the data-level, and already
+// * acked at the subflow level? */
+//if (m_dsn < tp->t_mpcb->ds_rcv_nxt &&
+//    M_TCPHDR(mq)->th_seq < tp->rcv_nxt) {
+//	tp->t_segq = mq->m_nextpkt;
+//	tp->t_segqlen -= mq->m_pkthdr.len;
+//	m_freem(mq);
+//	continue;
+//}
+
diff -r 1d1c4c997b66 sys/netinet/tcp_subr.c
--- a/sys/netinet/tcp_subr.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_subr.c	Sun Aug 30 14:27:42 2015 +1000
@@ -42,9 +42,12 @@
 #include <sys/systm.h>
 #include <sys/callout.h>
 #include <sys/hhook.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/khelp.h>
+#include <sys/limits.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 #include <sys/jail.h>
 #include <sys/malloc.h>
 #include <sys/mbuf.h>
@@ -53,6 +56,7 @@
 #endif
 #include <sys/priv.h>
 #include <sys/proc.h>
+#include <sys/queue.h>
 #include <sys/sdt.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
@@ -92,6 +96,9 @@
 #include <netinet6/tcp6_var.h>
 #endif
 #include <netinet/tcpip.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
+#include <crypto/sha1.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
 #endif
@@ -166,6 +173,11 @@
    "Default TCP Maximum Segment Size for IPv6");
 #endif /* INET6 */
 
+VNET_DEFINE(unsigned int, tcp_override_isn) = 0;
+SYSCTL_UINT(_net_inet_tcp, OID_AUTO, override_isn, CTLFLAG_RW,
+    &VNET_NAME(tcp_override_isn), 0,
+    "Manually set the initial sequence number of TCP flows");
+
 /*
  * Minimum MSS we accept and use. This prevents DoS attacks where
  * we are forced to a ridiculous low MSS like 20 and send hundreds
@@ -215,6 +227,12 @@
 SYSCTL_INT(_net_inet_tcp, OID_AUTO, soreceive_stream, CTLFLAG_RDTUN,
     &tcp_soreceive_stream, 0, "Using soreceive_stream for TCP sockets");
 
+VNET_DEFINE(int, tcp_do_mptcp) = 1;
+SYSCTL_INT(_net_inet_tcp, OID_AUTO, mptcp, CTLFLAG_RW,
+    &VNET_NAME(tcp_do_mptcp), 0,
+    "Enable Multipath Support");
+
+
 #ifdef TCP_SIGNATURE
 static int	tcp_sig_checksigs = 1;
 SYSCTL_INT(_net_inet_tcp, OID_AUTO, signature_verify_input, CTLFLAG_RW,
@@ -373,6 +391,10 @@
 	uma_zone_set_max(V_tcpcb_zone, maxsockets);
 	uma_zone_set_warning(V_tcpcb_zone, "kern.ipc.maxsockets limit reached");
 
+	/* mptcp init */
+	mpp_init();
+	mp_init();
+
 	tcp_tw_init();
 	syncache_init();
 	tcp_hc_init();
@@ -436,6 +458,8 @@
 	syncache_destroy();
 	tcp_tw_destroy();
 	in_pcbinfo_destroy(&V_tcbinfo);
+	mp_destroy();
+
 	uma_zdestroy(V_sack_hole_zone);
 	uma_zdestroy(V_tcpcb_zone);
 
@@ -809,6 +833,10 @@
 	in_pcbref(inp);	/* Reference for tcpcb */
 	tp->t_inpcb = inp;
 
+	/* dss map queue */
+	TAILQ_INIT(&tp->t_send_maps.dsmap_list);
+	TAILQ_INIT(&tp->t_rcv_maps.dsmap_list);
+
 	/*
 	 * Init srtt to TCPTV_SRTTBASE (0), so we can tell that we have no
 	 * rtt estimate.  Set rttvar so that srtt + 4 * rttvar gives
@@ -827,8 +855,36 @@
 	 * which may match an IPv4-mapped IPv6 address.
 	 */
 	inp->inp_ip_ttl = V_ip_defttl;
-	inp->inp_ppcb = tp;
-	return (tp);		/* XXX */
+	inp->inp_ppcb = tp;	/* XXX */
+	return (tp);
+}
+
+/* The mappings always start from beginning of the send buffer, and the
+ * maps are placed into the list in sequence order. */
+void
+dsmap_drop(struct dsmapq_head *dsmap_list, tcp_seq th_ack, int len)
+{
+	struct ds_map *map;
+
+    map = TAILQ_FIRST(dsmap_list);
+    while (len > 0) {
+    	KASSERT(map != NULL, ("%s: no send map, len %d", __func__, len));
+    	KASSERT(map->ds_map_remain >= len || TAILQ_NEXT(map, sf_ds_map_next),
+    	        ("%s: drop len %d greater than mapped bytes\n", __func__, len));
+
+		if (map->ds_map_remain > len) {
+			map->ds_map_remain -= len;
+			//map->sf_seq_start = th_ack;
+			break;
+		}
+		len -= map->ds_map_remain;
+
+		struct ds_map *n;
+		n = TAILQ_NEXT(map, sf_ds_map_next);
+		TAILQ_REMOVE(dsmap_list, map, sf_ds_map_next);
+		free(map, M_DSSMAP);
+		map = n;
+	}
 }
 
 /*
@@ -895,6 +951,55 @@
 }
 
 /*
+ * Switch CC algorithm to the passed-in algorithm. Should verify that the algo
+ * exists and the module is loaded before calling this function.
+ *
+ * XXXNJW: Not sure what happens in terms of the new algo initialising in
+ * 'slow-start' phase or similar. Perhaps the algorithms will sort themselves
+ * out using the data passed in by cc_var.
+ */
+//int
+//tcp_ccalgoswitch(struct cc_algo *new_algo, struct tcpcb *tp)
+//{
+//    struct inpcb *inp = tp->t_inpcb;
+//    struct cc_algo *old_algo;
+//    struct cc_var new_ccv;
+//
+//    INP_WLOCK(inp);
+//
+//    if (!(inp->inp_flags & INP_TIMEWAIT)) {
+//    	old_algo = CC_ALGO(tp);
+//
+//    	/* Attempt to initialise the new algorithm before switching. Failure
+//    	 * to initialise cc_data for the new CC algorithm will result in
+//    	 * returning with the original CC algorithm still configured. */
+//		if (new_algo->cb_init != NULL) {
+//			if (new_algo->cb_init(&new_ccv) > 0) {
+//				INP_WUNLOCK(inp);
+//				return ENOMEM;
+//			}
+//
+//			/* Remove any memory allocated by the previous CC algorithm. This
+//			 * ONLY clears memory pointed to by cc_data in the cc_var struct.
+//			 * i.e. all other ccv data will stay the same.
+//			 */
+//			if (old_algo->cb_destroy != NULL)
+//				old_algo->cb_destroy(tp->ccv);
+//
+//			/* Set pointer to newly initialised cc_data */
+//			tp->ccv->cc_data = new_ccv.cc_data;
+//		}
+//
+//		/* Switch over to the new algorithm */
+//		CC_ALGO(tp) = new_algo;
+//    }
+//
+//    INP_WUNLOCK(inp);
+//
+//    return 0;
+//}
+
+/*
  * Drop a TCP connection, reporting
  * the specified error.  If connection is synchronized,
  * then send a RST to peer.
@@ -1001,9 +1106,21 @@
 		tcp_hc_update(&inp->inp_inc, &metrics);
 	}
 
-	/* free the reassembly queue, if any */
+	/* free the reassembly queue, if any
+	 * XXXNJW: If a subflow has called into tcp_discard, could we still need
+	 * to retain the segments, in cases where there is some disorder and these
+	 * segments will be needed to reassemble later? */
 	tcp_reass_flush(tp);
 
+	/* free ds_maps from t_rxmaps */
+	struct ds_map *n1, *n2;
+	n1 = TAILQ_FIRST(&tp->t_rcv_maps.dsmap_list);
+    while (n1 != NULL) {
+            n2 = TAILQ_NEXT(n1, sf_ds_map_next);
+            free(n1, M_DSSMAP);
+            n1 = n2;
+    }
+
 #ifdef TCP_OFFLOAD
 	/* Disconnect offload device, if any. */
 	if (tp->t_flags & TF_TOE)
diff -r 1d1c4c997b66 sys/netinet/tcp_syncache.c
--- a/sys/netinet/tcp_syncache.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_syncache.c	Sun Aug 30 14:27:42 2015 +1000
@@ -41,6 +41,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
+#include <sys/endian.h>
 #include <sys/sysctl.h>
 #include <sys/limits.h>
 #include <sys/lock.h>
@@ -53,6 +54,7 @@
 #include <sys/socketvar.h>
 #include <sys/syslog.h>
 #include <sys/ucred.h>
+#include <sys/queue.h>
 
 #include <sys/md5.h>
 #include <crypto/siphash/siphash.h>
@@ -78,12 +80,19 @@
 #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/tcp_syncache.h>
+
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
+
 #ifdef INET6
 #include <netinet6/tcp6_var.h>
 #endif
@@ -211,6 +220,9 @@
 #define	SCH_UNLOCK(sch)		mtx_unlock(&(sch)->sch_mtx)
 #define	SCH_LOCK_ASSERT(sch)	mtx_assert(&(sch)->sch_mtx, MA_OWNED)
 
+#define lsototcpcb(lso) intotcpcb(((struct mpcb *)mpptompcb((struct mppcb *) \
+     sotomppcb(lso)))->m_cb_ref.inp)
+
 /*
  * Requires the syncache entry to be already removed from the bucket list.
  */
@@ -661,15 +673,25 @@
 	struct tcpcb *tp;
 	int error;
 	char *s;
+//	struct mppcb *l_mpp;
+	struct mppcb *mpp;
+	struct mpcb *mp;
+    struct socket *sf_so = NULL;
 
 	INP_INFO_WLOCK_ASSERT(&V_tcbinfo);
 
+//	l_mpp = sotomppcb(lso);
+//	MPP_LOCK_ASSERT(l_mpp);
+
 	/*
 	 * Ok, create the full blown connection, and set things up
 	 * as they would have been set up if we had created the
 	 * connection when the SYN arrived.  If we can't create
 	 * the connection, abort it.
 	 */
+	// XXXNJW: Calling sonewconn should be done with the MPP lock held. At this
+	// a usrreq might be called that tries to manipulate the socket or the MPP
+	// in some way.
 	so = sonewconn(lso, 0);
 	if (so == NULL) {
 		/*
@@ -686,6 +708,74 @@
 		}
 		goto abort2;
 	}
+
+	// at this point so is a duplicate of the listen socket. need to duplicate
+	// the mpcb fields from syncache mptcp fields to the new mpcb
+    // causes a bunch of witness LOR warnings, but shouldn't be a problem as
+	// the mpp and mp are held under the INP_WLOCK of an inp attached to a
+	// different mpcb
+
+	// should put the mppcb socket into "acceptconn" state, so that when the
+	// subflow triggers a "subflow_connected" event (on return to
+	// tcp_input), the mppcb socket also calls soisconencted in response,
+	// which leads to a wakeup of head->so_timeo, and the mppcb socket being
+	// dequeued from so_comp and "accept" being called on the mppcb socket.
+	// at this point the fd for the newly created mppcb socket will be
+	// returned to the process, and the process can start writing/receiving
+	// data on the new (MPTCP) socket.
+
+	// The following creates a LOR with the INP_INFO_WLOCK. Will need to do
+	// some un-locking/re-locking to make sure that the ordering is correct
+	// (TODO).
+
+	mpp = sotomppcb(so);
+	MPP_LOCK(mpp);
+    mp = mpptompcb(mpp);
+
+    /* Copy sequence numbers from the syncache. These are over-ridden
+     * if the connection transitions into MP */
+    //    mp_syncache_newmpcb(mp, sc);
+    mp->ds_rcv_nxt = sc->sc_irs + 1;
+    mp->ds_snd_una = mp->ds_map_max = mp->ds_map_min = mp->ds_snd_nxt =
+        sc->sc_iss + 1;
+    mp->mp_passive = 1;
+
+    /* create a socket for a new subflow */
+    error = mp_create_subflow_socket(so, &sf_so);
+	if (error) {
+		MPP_UNLOCK(mpp);
+		goto abort2;
+	}
+	KASSERT(sf_so != NULL, ("%s: subflow socket NULL", __func__));
+
+	/* Must mark the primary socket as connecting while waiting for the subflow
+	 * socket to reach connected and trigger a connected event. the primary
+     * socket will then call soisconnected and alloc a new fd for the process
+     * to use. */
+    if (!error)
+    	soisconnecting(so);
+
+    /* If an MP_CAPABLE exchange was made, enable MPTCP now. */
+//    if (sc->sc_mp_flags & MPF_MP_CAPABLE_ACK) {
+//    	sc->sc_mp_flags &= ~MPF_MP_CAPABLE_ACK;
+//    	soisconnected(so);
+//    	mp_init_established(mp);
+//    }
+
+	/* attach a tcpcb and inpcb to the subflow socket */
+	error = mp_attach_subflow(sf_so);
+
+	/* Insert the new sufblow pcbs and gso into sf_list */
+	error = mp_insert_subflow(mp, sf_so);
+
+    MPP_UNLOCK(mpp);
+
+	if (error)
+		goto abort2;
+
+    /* from here on work with the subflow socket */
+    so = sf_so;
+
 #ifdef MAC
 	mac_socketpeer_set_from_mbuf(m, so);
 #endif
@@ -846,9 +936,23 @@
 	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)
+	/* Will be copied to mpcb when mp_init_establish is called */
+	if (sc->sc_mp_flags & MPF_MP_CAPABLE_ACK) {
+		sc->sc_mp_flags &= ~MPF_MP_CAPABLE_ACK;
+		tp->t_mp_conn.local_key = sc->sc_local_key;
+		tp->t_mp_conn.remote_key = sc->sc_remote_key;
+		tp->t_mp_conn.local_token = sc->sc_mp_local_token;
+		tp->t_mp_conn.remote_token = sc->sc_mp_remote_token;
+		tp->t_mp_conn.ds_idss = sc->sc_ds_iss;
+		tp->t_mp_conn.ds_idrs = sc->sc_ds_irs;
+	}
+
+	//tp->t_flags = sototcpcb(lso)->t_flags & (TF_NOPUSH|TF_NODELAY);
+	tp->t_flags = lsototcpcb(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;
@@ -901,15 +1005,18 @@
 	/*
 	 * 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;
+	tp->t_keepinit = lsototcpcb(lso)->t_keepinit;
+	tp->t_keepidle = lsototcpcb(lso)->t_keepidle;
+	tp->t_keepintvl = lsototcpcb(lso)->t_keepintvl;
+	tp->t_keepcnt = lsototcpcb(lso)->t_keepcnt;
 	tcp_timer_activate(tp, TT_KEEP, TP_KEEPINIT(tp));
 
 	INP_WUNLOCK(inp);
 
-	soisconnected(so);
+	soisconnected(so); // XXXNJW: do we need this? the tcpcb returns in SYN_RECEIVED
+	                   // so the transport isn't actually connected at this point.
+	                   // and tcp_do_segment will mark the socket as connected once
+	                   // we hit the ACK_processing switch block.
 
 	TCPSTAT_INC(tcps_accepts);
 	return (so);
@@ -1061,6 +1168,9 @@
 		goto failed;
 	}
 
+	if (to->to_mopts.mpo_flags & MPOF_CAPABLE_ACK)
+		sc->sc_mp_flags |= MPF_MP_CAPABLE_ACK;
+
 	*lsop = syncache_socket(sc, *lsop, m);
 
 	if (*lsop == NULL)
@@ -1125,7 +1235,7 @@
 	 * soon as possible.
 	 */
 	so = *lsop;
-	tp = sototcpcb(so);
+	tp = lsototcpcb(so);
 	cred = crhold(so->so_cred);
 
 #ifdef INET6
@@ -1267,7 +1377,7 @@
 	sc->sc_todctx = todctx;
 #endif
 	sc->sc_irs = th->th_seq;
-	sc->sc_iss = arc4random();
+	sc->sc_iss = V_tcp_override_isn > 0 ? V_tcp_override_isn : arc4random();
 	sc->sc_flags = 0;
 	sc->sc_flowlabel = 0;
 
@@ -1330,6 +1440,26 @@
 	if (to->to_flags & TOF_SIGNATURE || ltflags & TF_SIGNATURE)
 		sc->sc_flags |= SCF_SIGNATURE;
 #endif
+
+	/* Set to 1 if we send an MP_CAPABLE option */
+	sc->sent_capable = 0;
+
+	/*
+	 * 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;
+			mp_syncache_process_remote_key(sc, to->to_mopts.remote_key);
+			mp_syncache_process_local_key(sc);
+			if (to->to_mopts.mpo_flags & MPOF_USE_CSUM)
+				sc->csum_enabled = 1;
+		} /* removed an else-if for MP_JOINs */
+	}
+
 	if (to->to_flags & TOF_SACKPERM)
 		sc->sc_flags |= SCF_SACK;
 	if (to->to_flags & TOF_MSS)
@@ -1504,6 +1634,19 @@
 			to.to_tsecr = sc->sc_tsreflect;
 			to.to_flags |= TOF_TS;
 		}
+		if(sc->sc_flags & SCF_MPTCP) {
+			to.to_mopts.mpo_flags = 0;
+			/* Got a SYN with MP_CAPABLE, so send MP_CAPABLE back */
+			if(sc->sc_mp_flags & MPF_MP_CAPABLE) {
+				to.to_flags |= TOF_MPTCP;
+				to.to_mopts.mpo_flags |=
+						MPOF_CAPABLE_SYN;
+				to.to_mopts.local_key = sc->sc_local_key;
+
+				if (sc->csum_enabled)
+					to.to_mopts.mpo_flags |= MPOF_USE_CSUM;
+			}
+		}
 		if (sc->sc_flags & SCF_SACK)
 			to.to_flags |= TOF_SACKPERM;
 #ifdef TCP_SIGNATURE
@@ -1531,8 +1674,15 @@
 			}
 		}
 #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;
@@ -2043,3 +2193,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 1d1c4c997b66 sys/netinet/tcp_syncache.h
--- a/sys/netinet/tcp_syncache.h	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_syncache.h	Sun Aug 30 14:27:42 2015 +1000
@@ -41,6 +41,8 @@
 void	 syncache_unreach(struct in_conninfo *, struct tcphdr *);
 int	 syncache_expand(struct in_conninfo *, struct tcpopt *,
 	     struct tcphdr *, struct socket **, struct mbuf *);
+int	 syncache_expand_subflow(struct in_conninfo *, struct tcpopt *,
+	     struct tcphdr *, struct socket **, struct mbuf *);
 void	 syncache_add(struct in_conninfo *, struct tcpopt *,
 	     struct tcphdr *, struct inpcb *, struct socket **, struct mbuf *,
 	     void *, void *);
@@ -77,6 +79,25 @@
 
 	void		*sc_pspare;		/* TCP_SIGNATURE */
 	u_int32_t	sc_spare[2];		/* UTO */
+
+/* MULTIPATH TCP */
+	uint64_t	sc_ds_iss;		/* initial data sequence num */
+	uint64_t	sc_ds_irs;		/* initial receive sequence num */
+	u_int64_t	sc_remote_key;
+	u_int64_t	sc_local_key;
+	uint32_t 	sc_local_rand;	/* random number for join MAC */
+	uint32_t 	sc_remote_rand;	/* random number for join MAC */
+	uint32_t	sc_mp_remote_token;	/* token for the multipath session */
+	uint32_t	sc_mp_local_token;	/* token for the multipath session */
+
+	uint8_t sc_hmac_local[20];	/* MP_JOIN handshakes use HMACs to ensure */
+	uint8_t sc_hmac_remote[20];	/* that a subflow belongs to connection */
+	char* 	sc_sentmac_ptr;		/* points to HMAC sent in mp_join ACK*/
+
+	uint8_t sent_capable:1,		/* sent mp_cable */
+			csum_enabled:1;
+	uint8_t	sc_mp_flags;		/* flags for mp subtype */
+	struct	mpcb *t_mpcb;		/* pointer to multipath cb */
 };
 
 /*
@@ -88,9 +109,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	16
+#define	SYNCOOKIE_LIFETIME	15		/* seconds */
+
+/*
+ * Flags for sc_mp_flags
+ */
+#define MPF_MP_CAPABLE	    0x01
+#define MPF_MP_CAPABLE_ACK	0x02
+#define MPF_MP_JOIN		    0x04
+
 struct syncache_head {
 	struct mtx	sch_mtx;
 	TAILQ_HEAD(sch_head, syncache)	sch_bucket;
@@ -100,9 +132,6 @@
 	struct tcp_syncache *sch_sc;
 };
 
-#define	SYNCOOKIE_SECRET_SIZE	16
-#define	SYNCOOKIE_LIFETIME	15		/* seconds */
-
 struct syncookie_secret {
 	volatile u_int oddeven;
 	uint8_t key[2][SYNCOOKIE_SECRET_SIZE];
diff -r 1d1c4c997b66 sys/netinet/tcp_timer.c
--- a/sys/netinet/tcp_timer.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_timer.c	Sun Aug 30 14:27:42 2015 +1000
@@ -48,6 +48,7 @@
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
 #include <sys/systm.h>
+#include <sys/taskqueue.h>
 
 #include <net/if.h>
 #include <net/route.h>
@@ -71,6 +72,8 @@
 #include <netinet6/tcp6_var.h>
 #endif
 #include <netinet/tcpip.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
 #endif
@@ -179,6 +182,22 @@
     "Path MTU Discovery IPv6 Black Hole Detection lowered MSS");
 #endif
 
+/* MPTCP */
+VNET_DECLARE(int, mpsubflowrexmits);
+#define	V_mpsubflowrexmits  VNET(mpsubflowrexmits)
+VNET_DEFINE(int, mpsubflowrexmits) = 8;
+SYSCTL_INT(_net_inet_tcp, OID_AUTO, mpsubflowrexmits,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(mpsubflowrexmits), 0,
+    "MPTCP - Max number of subflow-level rexmits");
+
+VNET_DECLARE(int, mprexmittrigger);
+#define	V_mprexmittrigger  VNET(mprexmittrigger)
+VNET_DEFINE(int, mprexmittrigger) = 3;
+SYSCTL_INT(_net_inet_tcp, OID_AUTO, mprexmittrigger,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(mprexmittrigger), 0,
+    "MPTCP - RTO count to trigger data-level rexmit");
+
+
 #ifdef	RSS
 static int	per_cpu_timers = 1;
 #else
@@ -549,9 +568,13 @@
 {
 	struct tcpcb *tp = xtp;
 	CURVNET_SET(tp->t_vnet);
+	struct mpcb *mp = tp->t_mpcb;
 	int rexmt;
 	int headlocked;
 	struct inpcb *inp;
+	uint64_t mp_rexmit_dsn;
+	int mp_rexmt = 0;
+
 #ifdef TCPDEBUG
 	int ostate;
 
@@ -585,8 +608,21 @@
 	 * Retransmission timer went off.  Message has not
 	 * been acked within retransmit interval.  Back off
 	 * to a longer retransmit interval and retransmit one segment.
+	 *
+	 * XXXNJW: added a (temporary) way to kill a retansmitting TCP
+	 * if we are using multipath. Should really be checking subflow
+	 * count however, as this will disconnect a single-subflow MPTCP
+	 * connection after rxtshift hits mpsubflowrexmits
 	 */
-	if (++tp->t_rxtshift > TCP_MAXRXTSHIFT) {
+	if (++tp->t_rxtshift > TCP_MAXRXTSHIFT ||
+	       ((tp->t_sf_state & SFS_MP_ENABLED) &&
+	       (tp->t_rxtshift == V_mpsubflowrexmits))) {
+
+		if (tp->t_sf_state & SFS_MP_ENABLED)
+			tp->t_sf_state |= SFS_MP_DISCONNECTED;
+
+		printf("%s: drop tp %p, sf state %d\n", __func__, tp, tp->t_sf_state);
+
 		tp->t_rxtshift = TCP_MAXRXTSHIFT;
 		TCPSTAT_INC(tcps_timeoutdrop);
 		in_pcbref(inp);
@@ -611,8 +647,10 @@
 		headlocked = 1;
 		goto out;
 	}
+
 	INP_INFO_RUNLOCK(&V_tcbinfo);
 	headlocked = 0;
+
 	if (tp->t_state == TCPS_SYN_SENT) {
 		/*
 		 * If the SYN was retransmitted, indicate CWND to be
@@ -779,6 +817,23 @@
 		tp->t_rttvar += (tp->t_srtt >> TCP_RTT_SHIFT);
 		tp->t_srtt = 0;
 	}
+
+	/*
+	 * After V_mprexmittrigger RTOs firing, re-inject the outstanding data.
+	 */
+	// triggered only once. all un-acked segments are enqueued at this time.
+	if (tp->t_state == TCPS_ESTABLISHED && (tp->t_sf_state & SFS_MP_ENABLED)
+		&& tp->t_rxtshift == V_mprexmittrigger) {
+		int map_offset;
+		struct ds_map *snd_dsmap = mp_find_dsmap(tp, tp->snd_nxt);
+		if (snd_dsmap != NULL) {
+			mp_rexmt = 1;
+   			map_offset = tp->snd_nxt - snd_dsmap->sf_seq_start;
+            mp_rexmit_dsn = snd_dsmap->ds_map_start + map_offset;
+		}
+	}
+
+
 	tp->snd_nxt = tp->snd_una;
 	tp->snd_recover = tp->snd_max;
 	/*
@@ -804,6 +859,24 @@
 		INP_WUNLOCK(inp);
 	if (headlocked)
 		INP_INFO_WUNLOCK(&V_tcbinfo);
+
+	/* Subflow has triggered multiple RTOs. Take the data-level
+	 * offset and resend via mp_output. mp_rexmt is only set when
+	 * we have a valid tp */
+	if (tp != NULL && mp_rexmt) {
+        MPP_LOCK(mp->mp_mppcb);
+        /* XXXNJW: To simplify for now, though this does work.
+         * If mp_state > ESTABLISHED, let the MP RTO take care of
+         * re-sending DFINs */
+        if (mp->mp_state == MPS_M_ESTABLISHED) {
+			mp->ds_snd_nxt = lmax(mp_rexmit_dsn, mp->ds_snd_una);
+			printf("%s: data-level re-inject at %u\n", __func__,
+				(uint32_t) mp->ds_snd_nxt);
+        }
+		(void) mp_output(mp);
+		MPP_UNLOCK(mp->mp_mppcb);
+	}
+
 	CURVNET_RESTORE();
 }
 
diff -r 1d1c4c997b66 sys/netinet/tcp_timewait.c
--- a/sys/netinet/tcp_timewait.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_timewait.c	Sun Aug 30 14:27:42 2015 +1000
@@ -39,6 +39,7 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/callout.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
 #include <sys/malloc.h>
@@ -47,6 +48,7 @@
 #include <sys/proc.h>
 #include <sys/socket.h>
 #include <sys/socketvar.h>
+#include <sys/taskqueue.h>
 #include <sys/protosw.h>
 #include <sys/random.h>
 
@@ -76,6 +78,8 @@
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
 #ifdef INET6
 #include <netinet6/tcp6_var.h>
 #endif
@@ -220,6 +224,8 @@
 void
 tcp_twstart(struct tcpcb *tp)
 {
+	printf("%s\n", __func__);
+
 	struct tcptw *tw;
 	struct inpcb *inp = tp->t_inpcb;
 	int acknow;
@@ -276,6 +282,7 @@
 			return;
 		}
 	}
+
 	/*
 	 * The tcptw will hold a reference on its inpcb until tcp_twclose
 	 * is called
@@ -350,9 +357,11 @@
 		ACCEPT_LOCK();
 		SOCK_LOCK(so);
 		so->so_state &= ~SS_PROTOREF;
+		printf("%s: calling sofree with discarded tp %p\n", __func__, tp);
 		sofree(so);
 	} else
 		INP_WUNLOCK(inp);
+
 }
 
 /*
@@ -453,6 +462,8 @@
 void
 tcp_twclose(struct tcptw *tw, int reuse)
 {
+	printf("%s\n", __func__);
+
 	struct socket *so;
 	struct inpcb *inp;
 
@@ -508,6 +519,7 @@
 		in_pcbfree(inp);
 	}
 	TCPSTAT_INC(tcps_closed);
+
 }
 
 static int
diff -r 1d1c4c997b66 sys/netinet/tcp_usrreq.c
--- a/sys/netinet/tcp_usrreq.c	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_usrreq.c	Sun Aug 30 14:27:42 2015 +1000
@@ -46,10 +46,17 @@
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/limits.h>
+#include <sys/endian.h>
+#include <sys/lock.h>
 #include <sys/malloc.h>
+#include <sys/mutex.h>
+#include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/sysctl.h>
 #include <sys/mbuf.h>
+#include <sys/queue.h>
+#include <sys/taskqueue.h>
+
 #ifdef INET6
 #include <sys/domain.h>
 #endif /* INET6 */
@@ -84,6 +91,11 @@
 #include <netinet/tcp_seq.h>
 #include <netinet/tcp_timer.h>
 #include <netinet/tcp_var.h>
+#include <netinet/tcp_usrreq.h>
+#include <netinet/mptcp.h>
+#include <netinet/mptcp_var.h>
+#include <netinet/mptcp_pcb.h>
+#include <netinet/mptcp_dtrace_declare.h>
 #include <netinet/tcpip.h>
 #ifdef TCPDEBUG
 #include <netinet/tcp_debug.h>
@@ -95,9 +107,8 @@
 /*
  * TCP protocol interface to socket abstraction.
  */
-static int	tcp_attach(struct socket *);
 #ifdef INET
-static int	tcp_connect(struct tcpcb *, struct sockaddr *,
+int	tcp_connect(struct tcpcb *, struct sockaddr *,
 		    struct thread *td);
 #endif /* INET */
 #ifdef INET6
@@ -105,7 +116,7 @@
 		    struct thread *td);
 #endif /* INET6 */
 static void	tcp_disconnect(struct tcpcb *);
-static void	tcp_usrclosed(struct tcpcb *);
+//static void	tcp_usrclosed(struct tcpcb *);
 static void	tcp_fill_info(struct tcpcb *, struct tcp_info *);
 
 #ifdef TCPDEBUG
@@ -123,7 +134,7 @@
  * TCP attaches to socket via pru_attach(), reserving space,
  * and an internet control block.
  */
-static int
+/*static */int
 tcp_usr_attach(struct socket *so, int proto, struct thread *td)
 {
 	struct inpcb *inp;
@@ -169,7 +180,9 @@
 	KASSERT(so->so_pcb == inp, ("tcp_detach: so_pcb != inp"));
 	KASSERT(inp->inp_socket == so, ("tcp_detach: inp_socket != so"));
 
-	tp = intotcpcb(inp);
+    SDT_PROBE1(mptcp, session, tcp_detach, entry, so);
+
+	tp = (inp->inp_ppcb == NULL) ? NULL : intotcpcb(inp);
 
 	if (inp->inp_flags & INP_TIMEWAIT) {
 		/*
@@ -228,6 +241,7 @@
 			INP_WUNLOCK(inp);
 		}
 	}
+
 }
 
 /*
@@ -237,7 +251,7 @@
  * which may finish later; embryonic TCB's can just
  * be discarded here.
  */
-static void
+/* static */ void
 tcp_usr_detach(struct socket *so)
 {
 	struct inpcb *inp;
@@ -248,6 +262,7 @@
 	INP_WLOCK(inp);
 	KASSERT(inp->inp_socket != NULL,
 	    ("tcp_usr_detach: inp_socket == NULL"));
+
 	tcp_detach(so, inp);
 	INP_INFO_WUNLOCK(&V_tcbinfo);
 }
@@ -256,7 +271,7 @@
 /*
  * Give the socket an address.
  */
-static int
+/* static */ int
 tcp_usr_bind(struct socket *so, struct sockaddr *nam, struct thread *td)
 {
 	int error = 0;
@@ -359,7 +374,7 @@
 /*
  * Prepare to accept connections.
  */
-static int
+/* static */ int
 tcp_usr_listen(struct socket *so, int backlog, struct thread *td)
 {
 	int error = 0;
@@ -452,7 +467,7 @@
  * Start keep-alive timer, and seed output sequence space.
  * Send initial segment on connection.
  */
-static int
+/* static */ int
 tcp_usr_connect(struct socket *so, struct sockaddr *nam, struct thread *td)
 {
 	int error = 0;
@@ -603,15 +618,18 @@
  *
  * SHOULD IMPLEMENT LATER PRU_CONNECT VIA REALLOC TCPCB.
  */
-static int
+/* static */ int
 tcp_usr_disconnect(struct socket *so)
 {
+	printf("%s\n", __func__);
+
 	struct inpcb *inp;
 	struct tcpcb *tp = NULL;
 	int error = 0;
 
 	TCPDEBUG0;
 	INP_INFO_WLOCK(&V_tcbinfo);
+
 	inp = sotoinpcb(so);
 	KASSERT(inp != NULL, ("tcp_usr_disconnect: inp == NULL"));
 	INP_WLOCK(inp);
@@ -635,8 +653,15 @@
 /*
  * Accept a connection.  Essentially all the work is done at higher levels;
  * just return the address of the peer, storing through addr.
+ *
+ * The rationale for acquiring the tcbinfo lock here is somewhat complicated,
+ * and is described in detail in the commit log entry for r175612.  Acquiring
+ * it delays an accept(2) racing with sonewconn(), which inserts the socket
+ * before the inpcb address/port fields are initialized.  A better fix would
+ * prevent the socket from being placed in the listen queue until all fields
+ * are fully initialized.
  */
-static int
+/* static */ int
 tcp_usr_accept(struct socket *so, struct sockaddr **nam)
 {
 	int error = 0;
@@ -651,6 +676,7 @@
 
 	inp = sotoinpcb(so);
 	KASSERT(inp != NULL, ("tcp_usr_accept: inp == NULL"));
+	INP_INFO_RLOCK(&V_tcbinfo);
 	INP_WLOCK(inp);
 	if (inp->inp_flags & (INP_TIMEWAIT | INP_DROPPED)) {
 		error = ECONNABORTED;
@@ -670,6 +696,7 @@
 out:
 	TCPDEBUG2(PRU_ACCEPT);
 	INP_WUNLOCK(inp);
+	INP_INFO_RUNLOCK(&V_tcbinfo);
 	if (error == 0)
 		*nam = in_sockaddr(port, &addr);
 	return error;
@@ -734,15 +761,18 @@
 /*
  * Mark the connection as being incapable of further output.
  */
-static int
+/* static */ int
 tcp_usr_shutdown(struct socket *so)
 {
+	printf("%s\n", __func__);
+
 	int error = 0;
 	struct inpcb *inp;
 	struct tcpcb *tp = NULL;
 
 	TCPDEBUG0;
 	INP_INFO_WLOCK(&V_tcbinfo);
+
 	inp = sotoinpcb(so);
 	KASSERT(inp != NULL, ("inp == NULL"));
 	INP_WLOCK(inp);
@@ -752,6 +782,7 @@
 	}
 	tp = intotcpcb(inp);
 	TCPDEBUG1();
+
 	socantsendmore(so);
 	tcp_usrclosed(tp);
 	if (!(inp->inp_flags & INP_DROPPED))
@@ -761,14 +792,13 @@
 	TCPDEBUG2(PRU_SHUTDOWN);
 	INP_WUNLOCK(inp);
 	INP_INFO_WUNLOCK(&V_tcbinfo);
-
 	return (error);
 }
 
 /*
  * After a receive, possibly send window update to peer.
  */
-static int
+/* static */ int
 tcp_usr_rcvd(struct socket *so, int flags)
 {
 	struct inpcb *inp;
@@ -788,7 +818,6 @@
 #ifdef TCP_OFFLOAD
 	if (tp->t_flags & TF_TOE)
 		tcp_offload_rcvd(tp);
-	else
 #endif
 	tcp_output(tp);
 
@@ -805,7 +834,7 @@
  * must either enqueue them or free them.  The other pru_* routines
  * generally are caller-frees.
  */
-static int
+/* static */ int
 tcp_usr_send(struct socket *so, int flags, struct mbuf *m,
     struct sockaddr *nam, struct mbuf *control, struct thread *td)
 {
@@ -984,7 +1013,7 @@
 /*
  * Abort the TCP.  Drop the connection abruptly.
  */
-static void
+/* static */ void
 tcp_usr_abort(struct socket *so)
 {
 	struct inpcb *inp;
@@ -1022,9 +1051,11 @@
 /*
  * TCP socket is closed.  Start friendly disconnect.
  */
-static void
+/* static */ void
 tcp_usr_close(struct socket *so)
 {
+	printf("%s\n", __func__);
+
 	struct inpcb *inp;
 	struct tcpcb *tp = NULL;
 	TCPDEBUG0;
@@ -1033,16 +1064,17 @@
 	KASSERT(inp != NULL, ("tcp_usr_close: inp == NULL"));
 
 	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);
@@ -1061,7 +1093,7 @@
 /*
  * Receive out-of-band data.
  */
-static int
+/* static */ int
 tcp_usr_rcvoob(struct socket *so, struct mbuf *m, int flags)
 {
 	int error = 0;
@@ -1157,7 +1189,7 @@
  * truncate the previous TIME-WAIT state and proceed.
  * Initialize connection parameters and enter SYN-SENT state.
  */
-static int
+/* static */ int
 tcp_connect(struct tcpcb *tp, struct sockaddr *nam, struct thread *td)
 {
 	struct inpcb *inp = tp->t_inpcb, *oinp;
@@ -1169,6 +1201,8 @@
 	INP_WLOCK_ASSERT(inp);
 	INP_HASH_WLOCK(&V_tcbinfo);
 
+	printf("%s: tp %p\n", __func__, tp);
+
 	if (inp->inp_lport == 0) {
 		error = in_pcbbind(inp, (struct sockaddr *)0, td->td_ucred);
 		if (error)
@@ -1205,7 +1239,7 @@
 
 	soisconnecting(so);
 	TCPSTAT_INC(tcps_connattempt);
-	tcp_state_change(tp, TCPS_SYN_SENT);
+	tp->t_state = TCPS_SYN_SENT;
 	tp->iss = tcp_new_isn(tp);
 	tcp_sendseqinit(tp);
 
@@ -1335,6 +1369,7 @@
 	error = 0;
 	inp = sotoinpcb(so);
 	KASSERT(inp != NULL, ("tcp_ctloutput: inp == NULL"));
+
 	INP_WLOCK(inp);
 	if (sopt->sopt_level != IPPROTO_TCP) {
 #ifdef INET6
@@ -1385,7 +1420,6 @@
 			    sizeof optval);
 			if (error)
 				return (error);
-
 			INP_WLOCK_RECHECK(inp);
 			switch (sopt->sopt_name) {
 			case TCP_NODELAY:
@@ -1604,27 +1638,6 @@
 			INP_WUNLOCK(inp);
 			error = sooptcopyout(sopt, buf, TCP_CA_NAME_MAX);
 			break;
-		case TCP_KEEPIDLE:
-		case TCP_KEEPINTVL:
-		case TCP_KEEPINIT:
-		case TCP_KEEPCNT:
-			switch (sopt->sopt_name) {
-			case TCP_KEEPIDLE:
-				ui = tp->t_keepidle / hz;
-				break;
-			case TCP_KEEPINTVL:
-				ui = tp->t_keepintvl / hz;
-				break;
-			case TCP_KEEPINIT:
-				ui = tp->t_keepinit / hz;
-				break;
-			case TCP_KEEPCNT:
-				ui = tp->t_keepcnt;
-				break;
-			}
-			INP_WUNLOCK(inp);
-			error = sooptcopyout(sopt, &ui, sizeof(ui));
-			break;
 		default:
 			INP_WUNLOCK(inp);
 			error = ENOPROTOOPT;
@@ -1641,7 +1654,7 @@
  * internet protocol control block, tcp control block,
  * bufer space, and entering LISTEN state if to accept connections.
  */
-static int
+int
 tcp_attach(struct socket *so)
 {
 	struct tcpcb *tp;
@@ -1731,7 +1744,7 @@
  * for peer to send FIN or not respond to keep-alives, etc.
  * We can let the user exit from the close as soon as the FIN is acked.
  */
-static void
+/* static */ void
 tcp_usrclosed(struct tcpcb *tp)
 {
 
@@ -1745,7 +1758,7 @@
 #endif
 		/* FALLTHROUGH */
 	case TCPS_CLOSED:
-		tcp_state_change(tp, TCPS_CLOSED);
+		tp->t_state = TCPS_CLOSED;
 		tp = tcp_close(tp);
 		/*
 		 * tcp_close() should never return NULL here as the socket is
@@ -1761,11 +1774,11 @@
 		break;
 
 	case TCPS_ESTABLISHED:
-		tcp_state_change(tp, TCPS_FIN_WAIT_1);
+		tp->t_state = TCPS_FIN_WAIT_1;
 		break;
 
 	case TCPS_CLOSE_WAIT:
-		tcp_state_change(tp, TCPS_LAST_ACK);
+		tp->t_state = TCPS_LAST_ACK;
 		break;
 	}
 	if (tp->t_state >= TCPS_FIN_WAIT_2) {
diff -r 1d1c4c997b66 sys/netinet/tcp_usrreq.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sys/netinet/tcp_usrreq.h	Sun Aug 30 14:27:42 2015 +1000
@@ -0,0 +1,36 @@
+/*
+ * tcp_usrreq.h
+ *
+ *  Created on: 02/10/2014
+ *      Author: nwilliams
+ */
+
+#ifndef TCP_USRREQ_H_
+#define TCP_USRREQ_H_
+
+/* Called from tcp_usr_* and mp_usr_* functions */
+int	tcp_attach(struct socket *so);
+int	tcp_connect(struct tcpcb *, struct sockaddr *, struct thread *td);
+
+/* Called from mp_usrreq */
+int		tcp_usr_attach(struct socket *so, int proto, struct thread *td);
+int		tcp_usr_bind(struct socket *so, struct sockaddr *nam,
+		    struct thread *td);
+void	tcp_usr_detach(struct socket *so);
+int		tcp_usr_listen(struct socket *so, int backlog, struct thread *td);
+int		tcp_usr_connect(struct socket *so, struct sockaddr *nam,
+			struct thread *td);
+int		tcp_usr_disconnect(struct socket *so);
+int		tcp_usr_accept(struct socket *so, struct sockaddr **nam);
+int		tcp_usr_shutdown(struct socket *so);
+int		tcp_usr_rcvd(struct socket *so, int flags);
+int		tcp_usr_rcvoob(struct socket *so, struct mbuf *m, int flags);
+int		tcp_usr_send(struct socket *so, int flags, struct mbuf *m,
+			struct sockaddr *nam, struct mbuf *control, struct thread *td);
+void	tcp_usr_abort(struct socket *so);
+void 	tcp_usr_close(struct socket *so);
+
+/* TEMP: Only used for initial testing */
+void tcp_usrclosed(struct tcpcb *tp);
+
+#endif /* TCP_USRREQ_H_ */
diff -r 1d1c4c997b66 sys/netinet/tcp_var.h
--- a/sys/netinet/tcp_var.h	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/netinet/tcp_var.h	Sun Aug 30 14:27:42 2015 +1000
@@ -34,6 +34,7 @@
 #define _NETINET_TCP_VAR_H_
 
 #include <netinet/tcp.h>
+#include <netinet/mptcp_types.h>
 
 #ifdef _KERNEL
 #include <net/vnet.h>
@@ -86,6 +87,69 @@
 #define ND6_HINT(tp)
 #endif
 
+/* Used to map outgoing and incoming segments. Maintained in
+ * t_txmaps and t_rxmaps lists. mbuf_start and mbuf_offset
+ * are not used in the rx context.
+ */
+struct ds_map {
+	TAILQ_ENTRY(ds_map) sf_ds_map_next;
+	TAILQ_ENTRY(ds_map) mp_ds_map_next;
+	TAILQ_ENTRY(ds_map) mp_dup_map_next;
+	TAILQ_ENTRY(ds_map) rxmit_map_next;
+	uint64_t ds_map_start;	/* starting DSN of mapping */
+	uint32_t ds_map_len;	/* length of data sequence mapping */
+	uint32_t ds_map_remain;	/* length of map not acknowledged */
+	uint32_t ds_map_offset;	/* bytes sent/acked from this mapping */ // XXXNJW unused
+	tcp_seq	sf_seq_start; /* starting tcp seq num of mapping */
+	struct tcpcb *sf_tp; /* pointer to the sf that this was created for */		// might remove later
+	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);
+
+/* Used by subflows during connection set-up, to allow set-up
+ * without mp-level involvement (i.e. during MP_CAPABLE and
+ * MP_JOIN handshakes). The values can be passed to the
+ * mp-level once a handshake is complete. */
+struct mp_connection {
+	uint64_t ds_idss;       /* initial send seq num (data-level) */
+	uint64_t ds_idrs;       /* initial recv seq num (data-level) */
+
+	/* tokens, seq nums, windows */
+	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 */
+
+	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 */
+
+	uint8_t  hmac_local[20];   /* MP_JOIN handshakes use HMACs to ensure */
+	uint8_t  hmac_remote[20];  /* that a subflow belongs to connection */
+	uint64_t hmac_trun_remote; /* mp_join sender truncated mac */
+
+	/* Data-level sequence numbers as set by the mp-level, for
+	 * options such as DACK, DSS+DSN */
+	uint64_t ds_ack_num;    /* The DACK to be sent by this subflow */
+	uint64_t ds_snd_nxt;    /* ds-level snd_nxt for this subflow */
+	uint64_t ds_last_dsn;   /* last dsn received */
+	uint32_t ds_snd_dsn;    /*  */
+};
+
+
+/* This is what a subflow uses in place of a send buffer. */
+struct buffer_map {
+    struct dsmapq_head dsmap_list;	 /* data sequence maps */
+    uint32_t total_mapped_length;
+};
+
+/* for unmapped segments received */
+TAILQ_HEAD(sf_unmapped_queue_head, tseg_qent);
+
 /*
  * Tcp control block, one per tcp; fields:
  * Organized for 16 byte cacheline efficiency.
@@ -106,8 +170,8 @@
 
 	tcp_seq	snd_una;		/* send unacknowledged */
 	tcp_seq	snd_max;		/* highest sequence number sent;
-					 * used to recognize retransmits
-					 */
+					 	 	 * used to recognize retransmits
+					 	 	 */
 	tcp_seq	snd_nxt;		/* send next */
 	tcp_seq	snd_up;			/* send urgent pointer */
 
@@ -124,10 +188,10 @@
 	u_long	snd_wnd;		/* send window */
 	u_long	snd_cwnd;		/* congestion-controlled window */
 	u_long	snd_spare1;		/* unused */
-	u_long	snd_ssthresh;		/* snd_cwnd size threshold for
-					 * for slow start exponential to
-					 * linear switch
-					 */
+	u_long	snd_ssthresh;	/* snd_cwnd size threshold for
+					         * for slow start exponential to
+					         * linear switch
+					         */
 	u_long	snd_spare2;		/* unused */
 	tcp_seq	snd_recover;		/* for use in NewReno Fast Recovery */
 
@@ -208,6 +272,24 @@
 	uint32_t t_ispare[8];		/* 5 UTO, 3 TBD */
 	void	*t_pspare2[4];		/* 1 TCP_SIGNATURE, 3 TBD */
 	uint64_t _pad[6];		/* 6 TBD (1-2 CC/RTT?) */
+
+/* MPTCP */
+	struct mpcb *t_mpcb;
+	struct mbuf *t_segq_received;  /* segments acked at the subflow level */
+
+	struct buffer_map t_send_maps; /* outgoing maps */
+	struct buffer_map t_rcv_maps;  /* incoming maps */
+
+	u_int32_t t_sf_state;    /* mp-related state for subflows (connecting, active) */
+	u_int32_t t_sf_flags;  /* flags for this subflow */
+	u_int16_t t_event_flags;  /* Flags for subflow events (connected, etc..) */
+	uint8_t	t_event_pending:1; /* Subflow event occurred */
+	uint8_t	t_addrid;  /* Address ID of the subflow */
+
+	struct mp_connection t_mp_conn; /* mp connection-related vars */
+
+	/* XXX temp debugging vars */
+	uint32_t last_ack_processed; /* The last ack processed by mp_input task */
 };
 
 /*
@@ -278,6 +360,42 @@
 #define	TCP_SIG_SPI	0x1000
 #endif /* TCP_SIGNATURE */
 
+/* Structure to hold  */
+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 */
+	u_char      *dss_data_len_p; /* Pointer to data length in option char */
+	uint8_t		*dss_opts_f_p;	/* pointer to dss flags */
+
+	int		optlen;			/* use this to identify types in segment processing */
+	int		address_count;	/* addresses available for this session */
+	uint8_t	addr_id;		/* address id, used in mp_join */
+	char	*snd_mac;	/* senders MAC, 160-bit sha */
+};
+
 /*
  * Flags for PLPMTU handling, t_flags2
  */
@@ -301,7 +419,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 */
@@ -310,6 +429,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 */
 };
 
 /*
@@ -619,6 +739,8 @@
 VNET_DECLARE(int, path_mtu_discovery);
 VNET_DECLARE(int, tcp_do_rfc3465);
 VNET_DECLARE(int, tcp_abc_l_var);
+VNET_DECLARE(unsigned int, tcp_override_isn);
+#define	V_tcp_override_isn	VNET(tcp_override_isn)
 #define	V_tcb			VNET(tcb)
 #define	V_tcbinfo		VNET(tcbinfo)
 #define	V_tcp_mssdflt		VNET(tcp_mssdflt)
@@ -645,6 +767,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 *
@@ -749,6 +880,18 @@
 	th->th_urp = ntohs(th->th_urp);
 }
 
+/*
+#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)
+*/
+
+void tcp_ackmap(struct tcpcb *tp, uint32_t acked, uint64_t data_ack_num);
+void dsmap_drop(struct dsmapq_head *dsmap_list, tcp_seq th_ack, int len);
+void tcp_unmapped_init(void);
+void tcp_unmapped_flush(struct tcpcb *tp);
+
 #ifdef TCP_SIGNATURE
 static inline void
 tcp_fields_to_net(struct tcphdr *th)
diff -r 1d1c4c997b66 sys/sys/socketvar.h
--- a/sys/sys/socketvar.h	Tue Jul 07 20:31:09 2015 +0000
+++ b/sys/sys/socketvar.h	Sun Aug 30 14:27:42 2015 +1000
@@ -363,17 +363,23 @@
 int	solisten_proto_check(struct socket *so);
 struct socket *
 	sonewconn(struct socket *head, int connstatus);
-
+struct socket *
+	gsoalloc(struct vnet *vnet);
 
 int	sopoll(struct socket *so, int events, struct ucred *active_cred,
 	    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);
