Series comparison

[PATCH v7] includes "client-server" partial support for:
1. MPTCP cookie request from client (seems OK).
2. MPTCP cookie offering from server (seems OK).
3. MPTCP SYN+DATA+COOKIE from client (seems OK).
4. subsequent write + read on the opened socket (seems OK).

===Changes between v6 and v7
- Order of commits is changed.
- Code is refactored
===Pros/Cons of v7:
- little modifications of existing TCP code in linux kernel.
- relatively decreased size of the patch (in comparison with v6)
due to reuse of some functions in tcp_fastopen.c.
===Future work:
-Adress the appearance of "MPTCP FIN" as duplicated acks.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>

Dmytro Shytyi (11):
  Add separate fastopen.c file.
  add mptcp_stream_connect to protocol.h
  Initiator: MSG_FASTOPEN sendto(). request cookie
  rfree(), rmem_uncharge() prototypes to protocol.h
  Initiator: add locks() to mptcp_sendmsg_fastopen.
  add mptcp_setsockopt_fastopen
  mptfo variables for msk, options. Fix loop retrans
  Fix unxpctd val of subflow->map_seq(dscrd packet)
  Listener: Add received skb to msk
  mptcp_fastopen_add_skb() helpers (skb to msk)
  selftests: mptfo initiator/listener

include/net/tcp.h                             |  27 ++
 net/ipv4/tcp_fastopen.c                       |  38 +-
 net/ipv4/tcp_input.c                          |  20 +-
 net/mptcp/Makefile                            |   2 +-
 net/mptcp/fastopen.c                          | 331 ++++++++++++++++++
 net/mptcp/options.c                           |   3 +
 net/mptcp/protocol.c                          |  12 +-
 net/mptcp/protocol.h                          |  56 ++-
 net/mptcp/sockopt.c                           |   3 +
 net/mptcp/subflow.c                           |   8 +-
 tools/testing/selftests/net/mptcp/mptfo.sh    |  13 +
 .../selftests/net/mptcp/mptfo_initiator.c     |  41 +++
 .../selftests/net/mptcp/mptfo_listener.c      |  98 ++++++
 13 files changed, 614 insertions(+), 38 deletions(-)
 create mode 100644 net/mptcp/fastopen.c
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo.sh
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_initiator.c
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_listener.c

-- 
2.25.1

Add separate *.c file. Function prototypes are coming to protocol.h
(Suggestion of @palbeni (JUN 17))

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/Makefile   | 2 +-
 net/mptcp/fastopen.c | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)
 create mode 100644 net/mptcp/fastopen.c

We call mptcp_stream_connect() from mptcp_sendmsg_fastopen() in
fastopen.c

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/protocol.c | 4 ++--
 net/mptcp/protocol.h | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -XXX,XX +XXX,XX @@ static void mptcp_subflow_early_fallback(struct mptcp_sock *msk,
 	__mptcp_do_fallback(msk);
 }
 
-static int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr,
-				int addr_len, int flags)
+int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr,
+			 int addr_len, int flags)
 {
 	struct mptcp_sock *msk = mptcp_sk(sock->sk);
 	struct mptcp_subflow_context *subflow;
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_event(enum mptcp_event_type type, const struct mptcp_sock *msk,
 void mptcp_event_addr_announced(const struct sock *ssk, const struct mptcp_addr_info *info);
 void mptcp_event_addr_removed(const struct mptcp_sock *msk, u8 id);
 bool mptcp_userspace_pm_active(const struct mptcp_sock *msk);
+int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags);
 
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
 {
-- 
2.25.1

Initiator: MSG_FASTOPEN in sendto triggers the mptcp_sendsmg_fastopen.
It requests a MPTFO cookie.
Suggestion @palbeni(JAN 17): 'split the patch in several small one'.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 50 ++++++++++++++++++++++++++++++++++++++++++++
 net/mptcp/protocol.c |  4 ++--
 net/mptcp/protocol.h |  6 ++++++
 3 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@
  */
 
 #include "protocol.h"
+
+int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
+			   size_t len, struct mptcp_sock *msk,
+			   size_t *copied)
+{
+	const struct iphdr *iph;
+	struct ubuf_info *uarg;
+	struct sockaddr *uaddr;
+	struct sk_buff *skb;
+	struct tcp_sock *tp;
+	struct socket *ssk;
+	int ret;
+
+	ssk = __mptcp_nmpc_socket(msk);
+	if (unlikely(!ssk))
+		goto out_EFAULT;
+	skb = tcp_stream_alloc_skb(ssk->sk, 0, ssk->sk->sk_allocation, true);
+	if (unlikely(!skb))
+		goto out_EFAULT;
+	iph = ip_hdr(skb);
+	if (unlikely(!iph))
+		goto out_EFAULT;
+	uarg = msg_zerocopy_realloc(sk, len, skb_zcopy(skb));
+	if (unlikely(!uarg))
+		goto out_EFAULT;
+	uaddr = msg->msg_name;
+
+	tp = tcp_sk(ssk->sk);
+	if (unlikely(!tp))
+		goto out_EFAULT;
+	if (!tp->fastopen_req)
+		tp->fastopen_req = kzalloc(sizeof(*tp->fastopen_req),
+					   ssk->sk->sk_allocation);
+
+	if (unlikely(!tp->fastopen_req))
+		goto out_EFAULT;
+	tp->fastopen_req->data = msg;
+	tp->fastopen_req->size = len;
+	tp->fastopen_req->uarg = uarg;
+
+	/* requests a cookie */
+	ret = mptcp_stream_connect(sk->sk_socket, uaddr,
+				   msg->msg_namelen, msg->msg_flags);
+	if (!ret)
+		*copied = len;
+	return ret;
+out_EFAULT:
+	ret = -EFAULT;
+	return ret;
+}
diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -XXX,XX +XXX,XX @@ static int mptcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t len)
 	int ret = 0;
 	long timeo;
 
-	/* we don't support FASTOPEN yet */
+	/* we don't fully support FASTOPEN yet */
 	if (msg->msg_flags & MSG_FASTOPEN)
-		return -EOPNOTSUPP;
+		ret = mptcp_sendmsg_fastopen(sk, msg, len, msk, &copied);
 
 	/* silently ignore everything else */
 	msg->msg_flags &= MSG_MORE | MSG_DONTWAIT | MSG_NOSIGNAL;
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_event_addr_removed(const struct mptcp_sock *msk, u8 id);
 bool mptcp_userspace_pm_active(const struct mptcp_sock *msk);
 int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags);
 
+// Fast Open Mechanism functions begin
+int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
+			   size_t len, struct mptcp_sock *msk,
+			   size_t *copied);
+// Fast Open Mechanism functions end
+
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
 {
 	return READ_ONCE(msk->pm.addr_signal) &
-- 
2.25.1

These 2 functions required the mptcp_fastopen_add_skb() that is not in
protocol.c. Thus we export them to *.h and reuse in fastopen.c

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/protocol.c | 4 ++--
 net/mptcp/protocol.h | 2 ++
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -XXX,XX +XXX,XX @@ static void __mptcp_rmem_reclaim(struct sock *sk, int amount)
 	__sk_mem_reduce_allocated(sk, amount);
 }
 
-static void mptcp_rmem_uncharge(struct sock *sk, int size)
+void mptcp_rmem_uncharge(struct sock *sk, int size)
 {
 	struct mptcp_sock *msk = mptcp_sk(sk);
 	int reclaimable;
@@ -XXX,XX +XXX,XX @@ static void mptcp_rmem_uncharge(struct sock *sk, int size)
 		__mptcp_rmem_reclaim(sk, reclaimable);
 }
 
-static void mptcp_rfree(struct sk_buff *skb)
+void mptcp_rfree(struct sk_buff *skb)
 {
 	unsigned int len = skb->truesize;
 	struct sock *sk = skb->sk;
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_event_addr_announced(const struct sock *ssk, const struct mptcp_addr_
 void mptcp_event_addr_removed(const struct mptcp_sock *msk, u8 id);
 bool mptcp_userspace_pm_active(const struct mptcp_sock *msk);
 int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags);
+void mptcp_rmem_uncharge(struct sock *sk, int size);
+void mptcp_rfree(struct sk_buff *skb);
 
 // Fast Open Mechanism functions begin
 int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
-- 
2.25.1

Suggestion of @mmartineau : 'add locks' is implemented in this patch

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 	struct socket *ssk;
 	int ret;
 
+	lock_sock((struct sock *)msk);
 	ssk = __mptcp_nmpc_socket(msk);
 	if (unlikely(!ssk))
 		goto out_EFAULT;
@@ -XXX,XX +XXX,XX @@ int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 		goto out_EFAULT;
 	uaddr = msg->msg_name;
 
+	lock_sock(ssk->sk);
+
 	tp = tcp_sk(ssk->sk);
 	if (unlikely(!tp))
-		goto out_EFAULT;
+		goto out_lock_EFAULT;
 	if (!tp->fastopen_req)
 		tp->fastopen_req = kzalloc(sizeof(*tp->fastopen_req),
 					   ssk->sk->sk_allocation);
 
 	if (unlikely(!tp->fastopen_req))
-		goto out_EFAULT;
+		goto out_lock_EFAULT;
 	tp->fastopen_req->data = msg;
 	tp->fastopen_req->size = len;
 	tp->fastopen_req->uarg = uarg;
 
+	release_sock(ssk->sk);
+	release_sock((struct sock *)msk);
+
 	/* requests a cookie */
 	ret = mptcp_stream_connect(sk->sk_socket, uaddr,
 				   msg->msg_namelen, msg->msg_flags);
 	if (!ret)
 		*copied = len;
 	return ret;
+
+out_lock_EFAULT:
+	release_sock(ssk->sk);
 out_EFAULT:
+	release_sock((struct sock *)msk);
 	ret = -EFAULT;
 	return ret;
 }
-- 
2.25.1

Add set MPTFO socket option for MPTCP.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 28 ++++++++++++++++++++++++++++
 net/mptcp/protocol.h |  2 ++
 net/mptcp/sockopt.c  |  3 +++
 3 files changed, 33 insertions(+)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 	ret = -EFAULT;
 	return ret;
 }
+
+int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
+				      unsigned int optlen)
+{
+	struct sock *sk = (struct sock *)msk;
+	struct net *net = sock_net(sk);
+	int val;
+	int ret;
+
+	ret = 0;
+
+	if (copy_from_sockptr(&val, optval, sizeof(val)))
+		return -EFAULT;
+
+	lock_sock(sk);
+
+	if (val >= 0 && ((1 << sk->sk_state) & (TCPF_CLOSE |
+	    TCPF_LISTEN))) {
+		tcp_fastopen_init_key_once(net);
+		fastopen_queue_tune(sk, val);
+	} else {
+		ret = -EINVAL;
+	}
+
+	release_sock(sk);
+
+	return ret;
+}
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_rfree(struct sk_buff *skb);
 int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 			   size_t len, struct mptcp_sock *msk,
 			   size_t *copied);
+int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
+				      unsigned int optlen);
 // Fast Open Mechanism functions end
 
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
diff --git a/net/mptcp/sockopt.c b/net/mptcp/sockopt.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/sockopt.c
+++ b/net/mptcp/sockopt.c
@@ -XXX,XX +XXX,XX @@ static bool mptcp_supported_sockopt(int level, int optname)
 		case TCP_NOTSENT_LOWAT:
 		case TCP_TX_DELAY:
 		case TCP_INQ:
+		case TCP_FASTOPEN:
 			return true;
 		}
 
@@ -XXX,XX +XXX,XX @@ static int mptcp_setsockopt_sol_tcp(struct mptcp_sock *msk, int optname,
 		return mptcp_setsockopt_sol_tcp_nodelay(msk, optval, optlen);
 	case TCP_DEFER_ACCEPT:
 		return mptcp_setsockopt_sol_tcp_defer(msk, optval, optlen);
+	case TCP_FASTOPEN:
+		return mptcp_setsockopt_sol_tcp_fastopen(msk, optval, optlen);
 	}
 
 	return -EOPNOTSUPP;
-- 
2.25.1

Introduce mptfo variables for msk and options.
Also fixe the infinite retransmissions in the end of second session.
Suggestion @palbeni (SEP 1) during the meting to 'look at ack'

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 16 ++++++++++++++++
 net/mptcp/options.c  |  3 +++
 net/mptcp/protocol.h |  6 +++++-
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
 
 	return ret;
 }
+
+void mptcp_treat_hshake_ack_fastopen(struct mptcp_sock *msk, struct mptcp_subflow_context *subflow,
+				     struct mptcp_options_received mp_opt)
+{
+	u64 ack_seq;
+
+	if (mp_opt.suboptions & OPTIONS_MPTCP_MPC && mp_opt.is_mptfo && msk->is_mptfo) {
+		msk->can_ack = true;
+		msk->remote_key = mp_opt.sndr_key;
+		mptcp_crypto_key_sha(msk->remote_key, NULL, &ack_seq);
+		ack_seq++;
+		WRITE_ONCE(msk->ack_seq, ack_seq);
+		pr_debug("ack_seq=%llu sndr_key=%llu", msk->ack_seq, mp_opt.sndr_key);
+		atomic64_set(&msk->rcv_wnd_sent, ack_seq);
+	}
+}
diff --git a/net/mptcp/options.c b/net/mptcp/options.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/options.c
+++ b/net/mptcp/options.c
@@ -XXX,XX +XXX,XX @@ static void mptcp_parse_option(const struct sk_buff *skb,
 			ptr += 8;
 		}
 		if (opsize >= TCPOLEN_MPTCP_MPC_ACK) {
+			mp_opt->is_mptfo = 1;
 			mp_opt->rcvr_key = get_unaligned_be64(ptr);
 			ptr += 8;
 		}
@@ -XXX,XX +XXX,XX @@ bool mptcp_incoming_options(struct sock *sk, struct sk_buff *skb)
 		return sk->sk_state != TCP_CLOSE;
 
 	if (unlikely(mp_opt.suboptions != OPTION_MPTCP_DSS)) {
+		mptcp_treat_hshake_ack_fastopen(msk, subflow, mp_opt);
+
 		if ((mp_opt.suboptions & OPTION_MPTCP_FASTCLOSE) &&
 		    msk->local_key == mp_opt.rcvr_key) {
 			WRITE_ONCE(msk->rcv_fastclose, true);
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ struct mptcp_options_received {
 		echo:1,
 		backup:1,
 		deny_join_id0:1,
-		__unused:2;
+		is_mptfo:1,
+		__unused:1;
 	u8	join_id;
 	u64	thmac;
 	u8	hmac[MPTCPOPT_HMAC_LEN];
@@ -XXX,XX +XXX,XX @@ struct mptcp_sock {
 	bool		use_64bit_ack; /* Set when we received a 64-bit DSN */
 	bool		csum_enabled;
 	bool		allow_infinite_fallback;
+	bool		is_mptfo;
 	u8		mpc_endpoint_id;
 	u8		recvmsg_inq:1,
 			cork:1,
@@ -XXX,XX +XXX,XX @@ int mptcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 			   size_t *copied);
 int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
 				      unsigned int optlen);
+void mptcp_treat_hshake_ack_fastopen(struct mptcp_sock *msk, struct mptcp_subflow_context *subflow,
+				     struct mptcp_options_received mp_opt);
 // Fast Open Mechanism functions end
 
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
-- 
2.25.1

Add the received skb on the listener side to msk and set flag mptfo to 1
to treat some parts only in MPTFO case.
This function is called from the functions presented in the one of the
patch of this serie (mptcp_add_skb helpers).

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 59 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ void mptcp_treat_hshake_ack_fastopen(struct mptcp_sock *msk, struct mptcp_subflo
 		atomic64_set(&msk->rcv_wnd_sent, ack_seq);
 	}
 }
+
+void mptcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb, struct request_sock *req)
+{
+	struct mptcp_subflow_context *subflow = mptcp_subflow_ctx(sk);
+	struct tcp_request_sock *tcp_r_sock = tcp_rsk(req);
+	struct sock *socket = mptcp_subflow_ctx(sk)->conn;
+	struct mptcp_sock *msk = mptcp_sk(socket);
+	struct tcp_sock *tp = tcp_sk(sk);
+
+	if (TCP_SKB_CB(skb)->end_seq == tp->rcv_nxt)
+		return;
+
+	skb = skb_clone(skb, GFP_ATOMIC);
+	if (!skb)
+		return;
+
+	skb_dst_drop(skb);
+
+	tp->segs_in = 0;
+	tcp_segs_in(tp, skb);
+	__skb_pull(skb, tcp_hdrlen(skb));
+	sk_forced_mem_schedule(sk, skb->truesize);
+
+	TCP_SKB_CB(skb)->seq++;
+	TCP_SKB_CB(skb)->tcp_flags &= ~TCPHDR_SYN;
+
+	tp->rcv_nxt = TCP_SKB_CB(skb)->end_seq;
+
+	msk->is_mptfo = 1;
+
+	//Solves: WARNING: at 704 _mptcp_move_skbs_from_subflow+0x5d0/0x651
+	tp->copied_seq += tp->rcv_nxt - tcp_r_sock->rcv_isn - 1;
+
+	subflow->map_seq = mptcp_subflow_get_mapped_dsn(subflow);
+
+	//Solves: BAD mapping: ssn=0 map_seq=1 map_data_len=3
+	subflow->ssn_offset = tp->copied_seq - 1;
+
+	skb_ext_reset(skb);
+
+	//mptcp_set_owner_r begin
+	skb_orphan(skb);
+	skb->sk = socket;
+	skb->destructor = mptcp_rfree;
+	atomic_add(skb->truesize, &socket->sk_rmem_alloc);
+	msk->rmem_fwd_alloc -= skb->truesize;
+	//mptcp_set owner_r end
+
+	__skb_queue_tail(&msk->receive_queue, skb);
+
+	atomic64_set(&msk->rcv_wnd_sent, mptcp_subflow_get_mapped_dsn(subflow));
+
+	tp->syn_data_acked = 1;
+
+	tp->bytes_received = skb->len;
+
+	if (TCP_SKB_CB(skb)->tcp_flags & TCPHDR_FIN)
+		tcp_fin(sk);
+}
-- 
2.25.1

Set of helpers for mptcp_skb_add(). Some functions are inspired from tcp
fastopen.c file. This chain helps to add “skb” to 
“&msk->sk_receive_queue”
Example: “subflow_v4_conn_request”->”mptcp_conn_request”->
”mptcp_try_fastopen”->”mptcp_fastopen_create_child”->
”mptcp_fastopen_add_skb”

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 include/net/tcp.h       |  27 +++++++
 net/ipv4/tcp_fastopen.c |  38 +++++-----
 net/ipv4/tcp_input.c    |  20 ++---
 net/mptcp/fastopen.c    | 163 ++++++++++++++++++++++++++++++++++++++++
 net/mptcp/protocol.h    |  39 ++++++++++
 net/mptcp/subflow.c     |   2 +-
 6 files changed, 261 insertions(+), 28 deletions(-)

diff --git a/include/net/tcp.h b/include/net/tcp.h
index XXXXXXX..XXXXXXX 100644
--- a/include/net/tcp.h
+++ b/include/net/tcp.h
@@ -XXX,XX +XXX,XX @@ int tcp_md5_hash_key(struct tcp_md5sig_pool *hp,
 		     const struct tcp_md5sig_key *key);
 
 /* From tcp_fastopen.c */
+struct sock *tcp_fastopen_create_child(struct sock *sk,
+				       struct sk_buff *skb,
+				       struct request_sock *req);
+bool tcp_fastopen_queue_check(struct sock *sk);
+int tcp_fastopen_get_cipher(struct net *net, struct inet_connection_sock *icsk,
+			    u64 *key);
+void tcp_fastopen_cookie_gen(struct sock *sk,
+			     struct request_sock *req,
+			     struct sk_buff *syn,
+			     struct tcp_fastopen_cookie *foc);
+int tcp_fastopen_cookie_gen_check(struct sock *sk,
+				  struct request_sock *req,
+				  struct sk_buff *syn,
+				  struct tcp_fastopen_cookie *orig,
+				  struct tcp_fastopen_cookie *valid_foc);
+bool tcp_fastopen_no_cookie(const struct sock *sk,
+			    const struct dst_entry *dst,
+			    int flag);
+void tcp_reqsk_record_syn(const struct sock *sk,
+			  struct request_sock *req,
+			  const struct sk_buff *skb);
+void tcp_ecn_create_request(struct request_sock *req,
+			    const struct sk_buff *skb,
+			    const struct sock *listen_sk,
+			    const struct dst_entry *dst);
+void tcp_openreq_init(struct request_sock *req,
+		      const struct tcp_options_received *rx_opt,
+		      struct sk_buff *skb, const struct sock *sk);
 void tcp_fastopen_cache_get(struct sock *sk, u16 *mss,
 			    struct tcp_fastopen_cookie *cookie);
 void tcp_fastopen_cache_set(struct sock *sk, u16 mss,
diff --git a/net/ipv4/tcp_fastopen.c b/net/ipv4/tcp_fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/ipv4/tcp_fastopen.c
+++ b/net/ipv4/tcp_fastopen.c
@@ -XXX,XX +XXX,XX @@
 #include <linux/tcp.h>
 #include <linux/rcupdate.h>
 #include <net/tcp.h>
+#include "../mptcp/protocol.h"
 
 void tcp_fastopen_init_key_once(struct net *net)
 {
@@ -XXX,XX +XXX,XX @@ static bool __tcp_fastopen_cookie_gen_cipher(struct request_sock *req,
 /* Generate the fastopen cookie by applying SipHash to both the source and
  * destination addresses.
  */
-static void tcp_fastopen_cookie_gen(struct sock *sk,
-				    struct request_sock *req,
-				    struct sk_buff *syn,
-				    struct tcp_fastopen_cookie *foc)
+void tcp_fastopen_cookie_gen(struct sock *sk,
+			     struct request_sock *req,
+			     struct sk_buff *syn,
+			     struct tcp_fastopen_cookie *foc)
 {
 	struct tcp_fastopen_context *ctx;
 
@@ -XXX,XX +XXX,XX @@ void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb)
 }
 
 /* returns 0 - no key match, 1 for primary, 2 for backup */
-static int tcp_fastopen_cookie_gen_check(struct sock *sk,
-					 struct request_sock *req,
-					 struct sk_buff *syn,
-					 struct tcp_fastopen_cookie *orig,
-					 struct tcp_fastopen_cookie *valid_foc)
+int tcp_fastopen_cookie_gen_check(struct sock *sk,
+				  struct request_sock *req,
+				  struct sk_buff *syn,
+				  struct tcp_fastopen_cookie *orig,
+				  struct tcp_fastopen_cookie *valid_foc)
 {
 	struct tcp_fastopen_cookie search_foc = { .len = -1 };
 	struct tcp_fastopen_cookie *foc = valid_foc;
@@ -XXX,XX +XXX,XX @@ static int tcp_fastopen_cookie_gen_check(struct sock *sk,
 	return ret;
 }
 
-static struct sock *tcp_fastopen_create_child(struct sock *sk,
-					      struct sk_buff *skb,
-					      struct request_sock *req)
+struct sock *tcp_fastopen_create_child(struct sock *sk,
+				       struct sk_buff *skb,
+				       struct request_sock *req)
 {
 	struct tcp_sock *tp;
 	struct request_sock_queue *queue = &inet_csk(sk)->icsk_accept_queue;
@@ -XXX,XX +XXX,XX @@ static struct sock *tcp_fastopen_create_child(struct sock *sk,
 
 	tp->rcv_nxt = TCP_SKB_CB(skb)->seq + 1;
 
-	tcp_fastopen_add_skb(child, skb);
+	if (sk_is_mptcp(sk))
+		mptcp_fastopen_add_skb(child, skb, req);
+	else
+		tcp_fastopen_add_skb(child, skb);
 
 	tcp_rsk(req)->rcv_nxt = tp->rcv_nxt;
 	tp->rcv_wup = tp->rcv_nxt;
@@ -XXX,XX +XXX,XX @@ static struct sock *tcp_fastopen_create_child(struct sock *sk,
 	return child;
 }
 
-static bool tcp_fastopen_queue_check(struct sock *sk)
+bool tcp_fastopen_queue_check(struct sock *sk)
 {
 	struct fastopen_queue *fastopenq;
 
@@ -XXX,XX +XXX,XX @@ static bool tcp_fastopen_queue_check(struct sock *sk)
 	return true;
 }
 
-static bool tcp_fastopen_no_cookie(const struct sock *sk,
-				   const struct dst_entry *dst,
-				   int flag)
+bool tcp_fastopen_no_cookie(const struct sock *sk,
+			    const struct dst_entry *dst,
+			    int flag)
 {
 	return (READ_ONCE(sock_net(sk)->ipv4.sysctl_tcp_fastopen) & flag) ||
 	       tcp_sk(sk)->fastopen_no_cookie ||
diff --git a/net/ipv4/tcp_input.c b/net/ipv4/tcp_input.c
index XXXXXXX..XXXXXXX 100644
--- a/net/ipv4/tcp_input.c
+++ b/net/ipv4/tcp_input.c
@@ -XXX,XX +XXX,XX @@ static inline void pr_drop_req(struct request_sock *req, __u16 port, int family)
  * RFC8311 §4.3 which updates RFC3168 to allow the development of such
  * extensions.
  */
-static void tcp_ecn_create_request(struct request_sock *req,
-				   const struct sk_buff *skb,
-				   const struct sock *listen_sk,
-				   const struct dst_entry *dst)
+void tcp_ecn_create_request(struct request_sock *req,
+			    const struct sk_buff *skb,
+			    const struct sock *listen_sk,
+			    const struct dst_entry *dst)
 {
 	const struct tcphdr *th = tcp_hdr(skb);
 	const struct net *net = sock_net(listen_sk);
@@ -XXX,XX +XXX,XX @@ static void tcp_ecn_create_request(struct request_sock *req,
 		inet_rsk(req)->ecn_ok = 1;
 }
 
-static void tcp_openreq_init(struct request_sock *req,
-			     const struct tcp_options_received *rx_opt,
-			     struct sk_buff *skb, const struct sock *sk)
+void tcp_openreq_init(struct request_sock *req,
+		      const struct tcp_options_received *rx_opt,
+		      struct sk_buff *skb, const struct sock *sk)
 {
 	struct inet_request_sock *ireq = inet_rsk(req);
 
@@ -XXX,XX +XXX,XX @@ static bool tcp_syn_flood_action(const struct sock *sk, const char *proto)
 	return want_cookie;
 }
 
-static void tcp_reqsk_record_syn(const struct sock *sk,
-				 struct request_sock *req,
-				 const struct sk_buff *skb)
+void tcp_reqsk_record_syn(const struct sock *sk,
+			  struct request_sock *req,
+			  const struct sk_buff *skb)
 {
 	if (tcp_sk(sk)->save_syn) {
 		u32 len = skb_network_header_len(skb) + tcp_hdrlen(skb);
diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ void mptcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb, struct request
 	if (TCP_SKB_CB(skb)->tcp_flags & TCPHDR_FIN)
 		tcp_fin(sk);
 }
+
+struct sock *mptcp_try_fastopen(struct sock *sk, struct sk_buff *skb,
+				struct request_sock *req,
+				struct tcp_fastopen_cookie *foc,
+				const struct dst_entry *dst)
+{
+	bool syn_data_status = TCP_SKB_CB(skb)->end_seq != TCP_SKB_CB(skb)->seq + 1;
+	struct tcp_fastopen_cookie valid_mptcp_foc = { .len = -1 };
+	struct sock *child_sock;
+	int ret = 0;
+
+	if ((syn_data_status || foc->len >= 0) &&
+	    tcp_fastopen_queue_check(sk)) {
+		foc->len = -1;
+		return NULL;
+	}
+
+	if (tcp_fastopen_no_cookie(sk, dst, TFO_SERVER_COOKIE_NOT_REQD))
+		goto fastopen;
+
+	if (foc->len == 0) {
+		tcp_fastopen_cookie_gen(sk, req, skb, &valid_mptcp_foc);
+	} else if (foc->len > 0) {
+		ret = tcp_fastopen_cookie_gen_check(sk, req, skb, foc,
+						    &valid_mptcp_foc);
+		if (ret) {
+fastopen:
+			child_sock = tcp_fastopen_create_child(sk, skb, req);
+			if (child_sock) {
+				if (ret == 2) {
+					valid_mptcp_foc.exp = foc->exp;
+					*foc = valid_mptcp_foc;
+				} else {
+					foc->len = -1;
+				}
+				return child_sock;
+			}
+		}
+	}
+	valid_mptcp_foc.exp = foc->exp;
+	*foc = valid_mptcp_foc;
+	return NULL;
+}
+
+int mptcp_conn_request(struct request_sock_ops *rsk_ops,
+		       const struct tcp_request_sock_ops *af_ops,
+		       struct sock *sk, struct sk_buff *skb)
+{
+	struct tcp_fastopen_cookie mptcp_foc = { .len = -1 };
+	struct tcp_options_received tmp_opt_rcvd;
+	__u32 isn = TCP_SKB_CB(skb)->tcp_tw_isn;
+	struct tcp_sock *tp_sock = tcp_sk(sk);
+	struct sock *mptcp_fo_sk = NULL;
+	struct net *net = sock_net(sk);
+	struct request_sock *req_sock;
+	bool want_cookie = false;
+	struct dst_entry *dst;
+	struct flowi fl;
+
+	if (sk_acceptq_is_full(sk))
+		goto drop;
+
+	req_sock = inet_reqsk_alloc(rsk_ops, sk, !want_cookie);
+	if (!req_sock)
+		goto drop;
+
+	req_sock->syncookie = want_cookie;
+	tcp_rsk(req_sock)->af_specific = af_ops;
+	tcp_rsk(req_sock)->ts_off = 1;
+	tcp_rsk(req_sock)->is_mptcp = 1;
+
+	tcp_clear_options(&tmp_opt_rcvd);
+	tmp_opt_rcvd.mss_clamp = af_ops->mss_clamp;
+	tmp_opt_rcvd.user_mss  = tp_sock->rx_opt.user_mss;
+	tcp_parse_options(sock_net(sk), skb, &tmp_opt_rcvd, 0,
+			  want_cookie ? NULL : &mptcp_foc);
+
+	if (want_cookie && !tmp_opt_rcvd.saw_tstamp)
+		tcp_clear_options(&tmp_opt_rcvd);
+
+	if (IS_ENABLED(CONFIG_SMC) && want_cookie)
+		tmp_opt_rcvd.smc_ok = 0;
+
+	tmp_opt_rcvd.tstamp_ok = 0;
+	tcp_openreq_init(req_sock, &tmp_opt_rcvd, skb, sk);
+	inet_rsk(req_sock)->no_srccheck = inet_sk(sk)->transparent;
+
+	inet_rsk(req_sock)->ir_iif = inet_request_bound_dev_if(sk, skb);
+
+	dst = af_ops->route_req(sk, skb, &fl, req_sock);
+	if (!dst)
+		goto drop_and_free;
+
+	if (tmp_opt_rcvd.tstamp_ok)
+		tcp_rsk(req_sock)->ts_off = af_ops->init_ts_off(net, skb);
+
+	if (!want_cookie && !isn) {
+		if (!net->ipv4.sysctl_tcp_syncookies &&
+		    (net->ipv4.sysctl_max_syn_backlog - inet_csk_reqsk_queue_len(sk) <
+		     (net->ipv4.sysctl_max_syn_backlog >> 2)) &&
+		    !tcp_peer_is_proven(req_sock, dst)) {
+			goto drop_and_release;
+		}
+
+		isn = af_ops->init_seq(skb);
+	}
+
+	tcp_ecn_create_request(req_sock, skb, sk, dst);
+
+	if (want_cookie) {
+		isn = cookie_init_sequence(af_ops, sk, skb, &req_sock->mss);
+		if (!tmp_opt_rcvd.tstamp_ok)
+			inet_rsk(req_sock)->ecn_ok = 0;
+	}
+
+	tcp_rsk(req_sock)->snt_isn = isn;
+	tcp_rsk(req_sock)->txhash = net_tx_rndhash();
+	tcp_rsk(req_sock)->syn_tos = TCP_SKB_CB(skb)->ip_dsfield;
+
+	tcp_openreq_init_rwin(req_sock, sk, dst);
+	sk_rx_queue_set(req_to_sk(req_sock), skb);
+	if (!want_cookie) {
+		tcp_reqsk_record_syn(sk, req_sock, skb);
+		mptcp_fo_sk = mptcp_try_fastopen(sk, skb, req_sock, &mptcp_foc, dst);
+	}
+	if (mptcp_fo_sk) {
+		af_ops->send_synack(mptcp_fo_sk, dst, &fl, req_sock,
+				    &mptcp_foc, TCP_SYNACK_FASTOPEN, skb);
+		if (!inet_csk_reqsk_queue_add(sk, req_sock, mptcp_fo_sk)) {
+			reqsk_fastopen_remove(mptcp_fo_sk, req_sock, false);
+			bh_unlock_sock(mptcp_fo_sk);
+			sock_put(mptcp_fo_sk);
+			goto drop_and_free;
+		}
+		sk->sk_data_ready(sk);
+		bh_unlock_sock(mptcp_fo_sk);
+		sock_put(mptcp_fo_sk);
+	} else {
+		tcp_rsk(req_sock)->tfo_listener = false;
+		if (!want_cookie) {
+			req_sock->timeout = tcp_timeout_init((struct sock *)req_sock);
+			inet_csk_reqsk_queue_hash_add(sk, req_sock, req_sock->timeout);
+		}
+		af_ops->send_synack(sk, dst, &fl, req_sock, &mptcp_foc,
+				    !want_cookie ? TCP_SYNACK_NORMAL :
+						   TCP_SYNACK_COOKIE,
+				    skb);
+		if (want_cookie) {
+			reqsk_free(req_sock);
+			return 0;
+		}
+	}
+	reqsk_put(req_sock);
+	return 0;
+
+drop_and_release:
+	dst_release(dst);
+drop_and_free:
+	__reqsk_free(req_sock);
+drop:
+	tcp_listendrop(sk);
+	return 0;
+}
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
 				      unsigned int optlen);
 void mptcp_treat_hshake_ack_fastopen(struct mptcp_sock *msk, struct mptcp_subflow_context *subflow,
 				     struct mptcp_options_received mp_opt);
+void mptcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb,
+			    struct request_sock *req);
+void mptcp_reqsk_record_syn(const struct sock *sk,
+			    struct request_sock *req,
+			    const struct sk_buff *skb);
+void mptcp_ecn_create_request(struct request_sock *req,
+			      const struct sk_buff *skb,
+			      const struct sock *listen_sk,
+			      const struct dst_entry *dst);
+void mptcp_openreq_init(struct request_sock *req,
+			const struct tcp_options_received *rx_opt,
+			struct sk_buff *skb, const struct sock *sk);
+struct sock *mptcp_fastopen_create_child(struct sock *sk,
+					 struct sk_buff *skb,
+					 struct request_sock *req);
+bool mptcp_fastopen_queue_check(struct sock *sk);
+bool mptcp_fastopen_cookie_gen_cipher(struct request_sock *req,
+				      struct sk_buff *syn,
+				      const siphash_key_t *key,
+				      struct tcp_fastopen_cookie *foc);
+void mptcp_fastopen_cookie_gen(struct sock *sk,
+			       struct request_sock *req,
+			       struct sk_buff *syn,
+			       struct tcp_fastopen_cookie *foc);
+int mptcp_fastopen_cookie_gen_check(struct sock *sk,
+				    struct request_sock *req,
+				    struct sk_buff *syn,
+				    struct tcp_fastopen_cookie *orig,
+				    struct tcp_fastopen_cookie *valid_foc);
+bool mptcp_fastopen_no_cookie(const struct sock *sk,
+			      const struct dst_entry *dst,
+			      int flag);
+struct sock *mptcp_try_fastopen(struct sock *sk, struct sk_buff *skb,
+				struct request_sock *req,
+				struct tcp_fastopen_cookie *foc,
+				const struct dst_entry *dst);
+int mptcp_conn_request(struct request_sock_ops *rsk_ops,
+		       const struct tcp_request_sock_ops *af_ops,
+		       struct sock *sk, struct sk_buff *skb);
 // Fast Open Mechanism functions end
 
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
diff --git a/net/mptcp/subflow.c b/net/mptcp/subflow.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/subflow.c
+++ b/net/mptcp/subflow.c
@@ -XXX,XX +XXX,XX @@ static int subflow_v4_conn_request(struct sock *sk, struct sk_buff *skb)
 	if (skb_rtable(skb)->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))
 		goto drop;
 
-	return tcp_conn_request(&mptcp_subflow_request_sock_ops,
+	return mptcp_conn_request(&mptcp_subflow_request_sock_ops,
 				&subflow_request_sock_ipv4_ops,
 				sk, skb);
 drop:
-- 
2.25.1

MPTFO tests: these are examples of initiator (sendto) and listener,
probably are going to be integrated to the mptcp_connect.* selftests

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 tools/testing/selftests/net/mptcp/mptfo.sh    | 13 +++
 .../selftests/net/mptcp/mptfo_initiator.c     | 41 ++++++++
 .../selftests/net/mptcp/mptfo_listener.c      | 98 +++++++++++++++++++
 3 files changed, 152 insertions(+)
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo.sh
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_initiator.c
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_listener.c

diff --git a/tools/testing/selftests/net/mptcp/mptfo.sh b/tools/testing/selftests/net/mptcp/mptfo.sh
new file mode 100644
index XXXXXXX..XXXXXXX
--- /dev/null
+++ b/tools/testing/selftests/net/mptcp/mptfo.sh
@@ -XXX,XX +XXX,XX @@
+#!/bin/bash
+#This is an example of environmen that was used to generate wireshark
+sudo ip netns add server
+sudo ip netns add client
+sudo ip link add veth0 type veth peer name veth1
+sudo ip link set veth1 netns server
+sudo ip link set veth0 netns client
+sudo ip netns exec client ip a a 10.10.0.1/24 dev veth0
+sudo ip netns exec server ip a a 10.10.0.2/24 dev veth1
+sudo ip netns exec client ip link set dev  veth0 up
+sudo ip netns exec server ip link set dev  veth1 up
+sudo ip netns exec server bash -c "echo 2 > /proc/sys/net/ipv4/tcp_fastopen"
+sudo ip netns exec client bash -c "echo 1 > /proc/sys/net/ipv4/tcp_fastopen"
diff --git a/tools/testing/selftests/net/mptcp/mptfo_initiator.c b/tools/testing/selftests/net/mptcp/mptfo_initiator.c
new file mode 100644
index XXXXXXX..XXXXXXX
--- /dev/null
+++ b/tools/testing/selftests/net/mptcp/mptfo_initiator.c
@@ -XXX,XX +XXX,XX @@
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <netinet/tcp.h>
+#include <string.h>
+#include <signal.h>
+
+#define SERVER_PORT 7003
+
+int main(int argc, char *argv[])
+{
+	unsigned char valsyn[3] = "abc";
+	struct sockaddr_in daddr;
+	char *valend = "fff";
+	char *val1 = "zz1";
+	char *val2 = "zz2";
+	char *val3 = "zz3";
+	int sock_fd = -1;
+	int ret;
+
+	memset(&daddr, 0, sizeof(daddr));
+	inet_pton(AF_INET, "10.10.0.2", &daddr.sin_addr);
+	daddr.sin_family = AF_INET;
+	daddr.sin_port = htons(SERVER_PORT);
+
+	sock_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_MPTCP);
+
+	ret = sendto(sock_fd, valsyn, 3, MSG_FASTOPEN, (struct sockaddr *) &daddr, sizeof(daddr));
+	ret = write(sock_fd, val1, 3);
+	ret = write(sock_fd, val2, 3);
+	ret = write(sock_fd, val2, 3);
+	ret = write(sock_fd, val2, 3);
+	ret = write(sock_fd, val3, 3);
+	ret = write(sock_fd, valend, 3);
+
+	close(sock_fd);
+	return EXIT_SUCCESS;
+}
diff --git a/tools/testing/selftests/net/mptcp/mptfo_listener.c b/tools/testing/selftests/net/mptcp/mptfo_listener.c
new file mode 100644
index XXXXXXX..XXXXXXX
--- /dev/null
+++ b/tools/testing/selftests/net/mptcp/mptfo_listener.c
@@ -XXX,XX +XXX,XX @@
+#include <arpa/inet.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <netinet/in.h>
+#include <linux/in.h>
+#include <netinet/tcp.h>
+
+#define CLIENT_QUEUE_LEN 10
+#define SERVER_PORT 7003
+
+int main(void)
+{
+	int listen_sock_fd = -1, client_sock_fd = -1;
+	char str_addr[INET6_ADDRSTRLEN];
+	struct sockaddr_in server_addr;
+	int ret, flag;
+	int qlen = 5;
+	char ch;
+
+	server_addr.sin_family = AF_INET;
+	inet_pton(AF_INET, "10.10.0.2", &server_addr.sin_addr);
+	server_addr.sin_port = htons(SERVER_PORT);
+
+	/* Create socket for listening (client requests) */
+	listen_sock_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_MPTCP);
+	if (listen_sock_fd == -1) {
+		perror("socket()server");
+		return EXIT_FAILURE;
+	}
+
+	/* Set socket to reuse address */
+	flag = 1;
+	ret = setsockopt(listen_sock_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(flag));
+	if (ret == -1) {
+		perror("setsockopt()");
+		return EXIT_FAILURE;
+	}
+
+	ret = setsockopt(listen_sock_fd, SOL_TCP, TCP_FASTOPEN, &qlen, sizeof(qlen));
+	if (ret == -1) {
+		perror("setsockopt()TCP_FASTOPEN");
+		return EXIT_FAILURE;
+	}
+
+	/* Bind address and socket together */
+	ret = bind(listen_sock_fd, (struct sockaddr *)&server_addr, sizeof(server_addr));
+	if (ret == -1) {
+		perror("bind()");
+		close(listen_sock_fd);
+		return EXIT_FAILURE;
+	}
+
+	/* Create listening queue (client requests) */
+	ret = listen(listen_sock_fd, CLIENT_QUEUE_LEN);
+	if (ret == -1) {
+		perror("listen()");
+		close(listen_sock_fd);
+		return EXIT_FAILURE;
+	}
+	perror("Server listening");
+	while (1) {
+		/* Do TCP handshake with client */
+		client_sock_fd = accept(listen_sock_fd,
+				NULL,
+				0);
+		if (client_sock_fd == -1) {
+			perror("accept()");
+			close(listen_sock_fd);
+			return EXIT_FAILURE;
+		} else {
+			perror("ACCEPT_SUCCESS");
+		}
+
+		char rb[1024];
+
+		while (1) {
+			ret = read(client_sock_fd, rb, 3);
+
+			if (ret == -1) {
+				perror("SERVVERread()");
+				close(client_sock_fd);
+				break;
+			} else {
+				fprintf(stderr, "received %c%c%c from client", rb[0], rb[1], rb[2]);
+			}
+			if (rb[0] == 'f' && rb[1] == 'f' && rb[2] == 'f') {
+				close(client_sock_fd);
+				break;
+			}
+
+		}
+	}
+
+	return EXIT_SUCCESS;
+}
-- 
2.25.1

[PATCH v8] includes "client-server" partial support for:
1. MPTCP cookie request from client (seems OK).
2. MPTCP cookie offering from server (seems OK).
3. MPTCP SYN+DATA+COOKIE from client (seems OK).
4. subsequent write + read on the opened socket (seems OK).
===Changes between v7 and v8
- We change from 0impact approach on existing TCP code to
  full reusage of existing and available functions.
- Code is refactored (Max. reuse of existing linux kernel code).
- fastopen.c is reduced to minima.
- Other comments from mailing list are coming in the next version.
===Future work
-Adress the appearance of "MPTCP FIN" as duplicated acks.
-Integrate presented in the last patch selftests.

Dmytro Shytyi (7):
  add mptcp_stream_connect to protocol.h
  add mptcp_setsockopt_fastopen
  reuse tcp_sendmsg_fastopen()
  mptfo variables for msk, options. Fix loop retrans
  Fix unxpctd val of subflow->map_seq(dscrd packet)
  add skb to mskq in tcp_fastopen_add_skb()
  selftests: mptfo initiator/listener

include/net/tcp.h                             |  5 +-
 net/ipv4/tcp.c                                | 18 +++-
 net/ipv4/tcp_fastopen.c                       | 55 +++++++++--
 net/ipv4/tcp_input.c                          | 11 ++-
 net/mptcp/Makefile                            |  2 +-
 net/mptcp/fastopen.c                          | 46 +++++++++
 net/mptcp/options.c                           |  9 ++
 net/mptcp/protocol.c                          | 19 ++--
 net/mptcp/protocol.h                          | 14 ++-
 net/mptcp/sockopt.c                           |  3 +
 tools/testing/selftests/net/mptcp/mptfo.sh    | 13 +++
 .../selftests/net/mptcp/mptfo_initiator.c     | 41 ++++++++
 .../selftests/net/mptcp/mptfo_listener.c      | 98 +++++++++++++++++++
 13 files changed, 311 insertions(+), 23 deletions(-)
 create mode 100644 net/mptcp/fastopen.c
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo.sh
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_initiator.c
 create mode 100644 tools/testing/selftests/net/mptcp/mptfo_listener.c

-- 
2.25.1

In the following patches we will call mptcp_stream_connect() from
function tcp_sendmsg_fastopen() in file "net/ipv4/tcp.c", thus make
such symbol visible.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/protocol.c | 4 ++--
 net/mptcp/protocol.h | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

Add set MPTFO socket option for MPTCP.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/Makefile   |  2 +-
 net/mptcp/fastopen.c | 32 ++++++++++++++++++++++++++++++++
 net/mptcp/protocol.h |  5 +++++
 net/mptcp/sockopt.c  |  3 +++
 4 files changed, 41 insertions(+), 1 deletion(-)
 create mode 100644 net/mptcp/fastopen.c

diff --git a/net/mptcp/Makefile b/net/mptcp/Makefile
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/Makefile
+++ b/net/mptcp/Makefile
@@ -XXX,XX +XXX,XX @@
 obj-$(CONFIG_MPTCP) += mptcp.o
 
 mptcp-y := protocol.o subflow.o options.o token.o crypto.o ctrl.o pm.o diag.o \
-	   mib.o pm_netlink.o sockopt.o pm_userspace.o sched.o
+	   mib.o pm_netlink.o sockopt.o pm_userspace.o sched.o fastopen.o
 
 obj-$(CONFIG_SYN_COOKIES) += syncookies.o
 obj-$(CONFIG_INET_MPTCP_DIAG) += mptcp_diag.o
diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
new file mode 100644
index XXXXXXX..XXXXXXX
--- /dev/null
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@
+/* SPDX-License-Identifier: GPL-2.0
+ * MPTCP Fast Open Mechanism. Copyright (c) 2021-2022, Dmytro SHYTYI
+ */
+
+#include "protocol.h"
+
+int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
+				      unsigned int optlen)
+{
+	struct sock *sk = (struct sock *)msk;
+	struct net *net = sock_net(sk);
+	int val;
+	int ret;
+
+	ret = 0;
+
+	if (copy_from_sockptr(&val, optval, sizeof(val)))
+		return -EFAULT;
+
+	lock_sock(sk);
+
+	if (val >= 0 && ((1 << sk->sk_state) & (TCPF_CLOSE | TCPF_LISTEN))) {
+		tcp_fastopen_init_key_once(net);
+		fastopen_queue_tune(sk, val);
+	} else {
+		ret = -EINVAL;
+	}
+
+	release_sock(sk);
+
+	return ret;
+}
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_event_addr_removed(const struct mptcp_sock *msk, u8 id);
 bool mptcp_userspace_pm_active(const struct mptcp_sock *msk);
 int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags);
 
+// Fast Open Mechanism functions begin
+int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
+				      unsigned int optlen);
+// Fast Open Mechanism functions end
+
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
 {
 	return READ_ONCE(msk->pm.addr_signal) &
diff --git a/net/mptcp/sockopt.c b/net/mptcp/sockopt.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/sockopt.c
+++ b/net/mptcp/sockopt.c
@@ -XXX,XX +XXX,XX @@ static bool mptcp_supported_sockopt(int level, int optname)
 		case TCP_NOTSENT_LOWAT:
 		case TCP_TX_DELAY:
 		case TCP_INQ:
+		case TCP_FASTOPEN:
 			return true;
 		}
 
@@ -XXX,XX +XXX,XX @@ static int mptcp_setsockopt_sol_tcp(struct mptcp_sock *msk, int optname,
 		return mptcp_setsockopt_sol_tcp_nodelay(msk, optval, optlen);
 	case TCP_DEFER_ACCEPT:
 		return mptcp_setsockopt_sol_tcp_defer(msk, optval, optlen);
+	case TCP_FASTOPEN:
+		return mptcp_setsockopt_sol_tcp_fastopen(msk, optval, optlen);
 	}
 
 	return -EOPNOTSUPP;
-- 
2.25.1

In the following patches we will reuse modified tcp_sendmsg_fastopen().
We call it from mptcp_sendmsg().

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 include/net/tcp.h    |  3 +++
 net/ipv4/tcp.c       | 18 +++++++++++++-----
 net/mptcp/protocol.c | 11 +++++++++--
 3 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/include/net/tcp.h b/include/net/tcp.h
index XXXXXXX..XXXXXXX 100644
--- a/include/net/tcp.h
+++ b/include/net/tcp.h
@@ -XXX,XX +XXX,XX @@ struct sock *tcp_try_fastopen(struct sock *sk, struct sk_buff *skb,
 			      struct request_sock *req,
 			      struct tcp_fastopen_cookie *foc,
 			      const struct dst_entry *dst);
+int tcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
+			 int *copied, size_t size,
+			 struct ubuf_info *uarg);
 void tcp_fastopen_init_key_once(struct net *net);
 bool tcp_fastopen_cookie_check(struct sock *sk, u16 *mss,
 			     struct tcp_fastopen_cookie *cookie);
diff --git a/net/ipv4/tcp.c b/net/ipv4/tcp.c
index XXXXXXX..XXXXXXX 100644
--- a/net/ipv4/tcp.c
+++ b/net/ipv4/tcp.c
@@ -XXX,XX +XXX,XX @@
 #include <asm/ioctls.h>
 #include <net/busy_poll.h>
 
+#include <net/mptcp.h>
+#include "../mptcp/protocol.h"
+
 /* Track pending CMSGs. */
 enum {
 	TCP_CMSG_INQ = 1,
@@ -XXX,XX +XXX,XX @@ void tcp_free_fastopen_req(struct tcp_sock *tp)
 	}
 }
 
-static int tcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
-				int *copied, size_t size,
-				struct ubuf_info *uarg)
+int tcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
+			 int *copied, size_t size,
+			 struct ubuf_info *uarg)
 {
 	struct tcp_sock *tp = tcp_sk(sk);
 	struct inet_sock *inet = inet_sk(sk);
@@ -XXX,XX +XXX,XX @@ static int tcp_sendmsg_fastopen(struct sock *sk, struct msghdr *msg,
 		}
 	}
 	flags = (msg->msg_flags & MSG_DONTWAIT) ? O_NONBLOCK : 0;
-	err = __inet_stream_connect(sk->sk_socket, uaddr,
-				    msg->msg_namelen, flags, 1);
+	if (!sk_is_mptcp(sk))
+		err = __inet_stream_connect(sk->sk_socket, uaddr,
+					    msg->msg_namelen, flags, 1);
+	else
+		err = mptcp_stream_connect(sk->sk_socket, uaddr,
+					   msg->msg_namelen, msg->msg_flags);
+
 	/* fastopen_req could already be freed in __inet_stream_connect
 	 * if the connection times out or gets rst
 	 */
diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -XXX,XX +XXX,XX @@ static int mptcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t len)
 	long timeo;
 
 	/* we don't support FASTOPEN yet */
-	if (msg->msg_flags & MSG_FASTOPEN)
-		return -EOPNOTSUPP;
+	if (msg->msg_flags & MSG_FASTOPEN) {
+		struct socket *ssock = __mptcp_nmpc_socket(msk);
 
+		if (ssock) {
+			int copied_syn_fastopen = 0;
+
+			ret = tcp_sendmsg_fastopen(ssock->sk, msg, &copied_syn_fastopen, len, NULL);
+			copied += copied_syn_fastopen;
+		}
+	}
 	/* silently ignore everything else */
 	msg->msg_flags &= MSG_MORE | MSG_DONTWAIT | MSG_NOSIGNAL;
 
-- 
2.25.1

Introduce mptfo variables for msk and options.
Also fix the infinite retransmissions in the end of second session.

The variable 2nd ack received in struct mptcp_options_received identifies the
received ack on the listener side during 3way handshake in mptfo context
and miningless alone if used alone. It is further used(checked)
in conjunction in the same "if" statement with variable is_mptfo
from struct mptcp_sock.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 net/mptcp/fastopen.c | 14 ++++++++++++++
 net/mptcp/options.c  |  4 ++++
 net/mptcp/protocol.h |  6 +++++-
 3 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/net/mptcp/fastopen.c b/net/mptcp/fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/fastopen.c
+++ b/net/mptcp/fastopen.c
@@ -XXX,XX +XXX,XX @@ int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
 
 	return ret;
 }
+
+void mptcp_gen_msk_ackseq_fastopen(struct mptcp_sock *msk, struct mptcp_subflow_context *subflow,
+				   struct mptcp_options_received mp_opt)
+{
+	u64 ack_seq;
+
+	msk->can_ack = true;
+	msk->remote_key = mp_opt.sndr_key;
+	mptcp_crypto_key_sha(msk->remote_key, NULL, &ack_seq);
+	ack_seq++;
+	WRITE_ONCE(msk->ack_seq, ack_seq);
+	pr_debug("ack_seq=%llu sndr_key=%llu", msk->ack_seq, mp_opt.sndr_key);
+	atomic64_set(&msk->rcv_wnd_sent, ack_seq);
+}
diff --git a/net/mptcp/options.c b/net/mptcp/options.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/options.c
+++ b/net/mptcp/options.c
@@ -XXX,XX +XXX,XX @@ static void mptcp_parse_option(const struct sk_buff *skb,
 			ptr += 8;
 		}
 		if (opsize >= TCPOLEN_MPTCP_MPC_ACK) {
+			mp_opt->hns_2nd_ack_rcvd = 1;
 			mp_opt->rcvr_key = get_unaligned_be64(ptr);
 			ptr += 8;
 		}
@@ -XXX,XX +XXX,XX @@ bool mptcp_incoming_options(struct sock *sk, struct sk_buff *skb)
 		return sk->sk_state != TCP_CLOSE;
 
 	if (unlikely(mp_opt.suboptions != OPTION_MPTCP_DSS)) {
+		if (mp_opt.suboptions & OPTIONS_MPTCP_MPC && mp_opt.hns_2nd_ack_rcvd && msk->is_mptfo)
+			mptcp_gen_msk_ackseq_fastopen(msk, subflow, mp_opt);
+
 		if ((mp_opt.suboptions & OPTION_MPTCP_FASTCLOSE) &&
 		    msk->local_key == mp_opt.rcvr_key) {
 			WRITE_ONCE(msk->rcv_fastclose, true);
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ struct mptcp_options_received {
 		echo:1,
 		backup:1,
 		deny_join_id0:1,
-		__unused:2;
+		__unused:1;
+	u8	hns_2nd_ack_rcvd:1;
 	u8	join_id;
 	u64	thmac;
 	u8	hmac[MPTCPOPT_HMAC_LEN];
@@ -XXX,XX +XXX,XX @@ struct mptcp_sock {
 	bool		use_64bit_ack; /* Set when we received a 64-bit DSN */
 	bool		csum_enabled;
 	bool		allow_infinite_fallback;
+	bool		is_mptfo;
 	u8		mpc_endpoint_id;
 	u8		recvmsg_inq:1,
 			cork:1,
@@ -XXX,XX +XXX,XX @@ int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_l
 // Fast Open Mechanism functions begin
 int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
 				      unsigned int optlen);
+void mptcp_gen_msk_ackseq_fastopen(struct mptcp_sock *msk, struct mptcp_subflow_context *subflow,
+				   struct mptcp_options_received mp_opt);
 // Fast Open Mechanism functions end
 
 static inline bool mptcp_pm_should_add_signal(struct mptcp_sock *msk)
-- 
2.25.1

In the following patches we add skb to msk->receive_queue in the MPTCP
fastopen context.

Signed-off-by: Dmytro Shytyi <dmytro@shytyi.net>
---
 include/net/tcp.h       |  2 +-
 net/ipv4/tcp_fastopen.c | 55 +++++++++++++++++++++++++++++++++++------
 net/ipv4/tcp_input.c    | 11 +++++++--
 net/mptcp/protocol.c    |  4 +--
 net/mptcp/protocol.h    |  2 ++
 5 files changed, 62 insertions(+), 12 deletions(-)

diff --git a/include/net/tcp.h b/include/net/tcp.h
index XXXXXXX..XXXXXXX 100644
--- a/include/net/tcp.h
+++ b/include/net/tcp.h
@@ -XXX,XX +XXX,XX @@ int tcp_fastopen_reset_cipher(struct net *net, struct sock *sk,
 			      void *primary_key, void *backup_key);
 int tcp_fastopen_get_cipher(struct net *net, struct inet_connection_sock *icsk,
 			    u64 *key);
-void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb);
+void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb, struct request_sock *req);
 struct sock *tcp_try_fastopen(struct sock *sk, struct sk_buff *skb,
 			      struct request_sock *req,
 			      struct tcp_fastopen_cookie *foc,
diff --git a/net/ipv4/tcp_fastopen.c b/net/ipv4/tcp_fastopen.c
index XXXXXXX..XXXXXXX 100644
--- a/net/ipv4/tcp_fastopen.c
+++ b/net/ipv4/tcp_fastopen.c
@@ -XXX,XX +XXX,XX @@
 #include <linux/tcp.h>
 #include <linux/rcupdate.h>
 #include <net/tcp.h>
+#include "../mptcp/protocol.h"
 
 void tcp_fastopen_init_key_once(struct net *net)
 {
@@ -XXX,XX +XXX,XX @@ static void tcp_fastopen_cookie_gen(struct sock *sk,
 /* If an incoming SYN or SYNACK frame contains a payload and/or FIN,
  * queue this additional data / FIN.
  */
-void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb)
+void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb, struct request_sock *req)
 {
+	struct mptcp_subflow_context *subflow = mptcp_subflow_ctx(sk);
+	struct tcp_request_sock *tcp_r_sock = tcp_rsk(req);
+	struct sock *socket = mptcp_subflow_ctx(sk)->conn;
+	struct mptcp_sock *msk = mptcp_sk(socket);
 	struct tcp_sock *tp = tcp_sk(sk);
 
 	if (TCP_SKB_CB(skb)->end_seq == tp->rcv_nxt)
@@ -XXX,XX +XXX,XX @@ void tcp_fastopen_add_skb(struct sock *sk, struct sk_buff *skb)
 	TCP_SKB_CB(skb)->tcp_flags &= ~TCPHDR_SYN;
 
 	tp->rcv_nxt = TCP_SKB_CB(skb)->end_seq;
+
+	if (req && tp->syn_fastopen && sk_is_mptcp(sk))
+		tcp_r_sock = tcp_rsk(req);
+	else
+		goto add_skb_to_sk;
+
+	msk->is_mptfo = 1;
+
+	//Solves: WARNING: at 704 _mptcp_move_skbs_from_subflow+0x5d0/0x651
+	tp->copied_seq += tp->rcv_nxt - tcp_r_sock->rcv_isn - 1;
+
+	subflow->map_seq = mptcp_subflow_get_mapped_dsn(subflow);
+
+	//Solves: BAD mapping: ssn=0 map_seq=1 map_data_len=3
+	subflow->ssn_offset = tp->copied_seq - 1;
+
+	skb_orphan(skb);
+	skb->sk = socket;
+	skb->destructor = mptcp_rfree;
+	atomic_add(skb->truesize, &socket->sk_rmem_alloc);
+	msk->rmem_fwd_alloc -= skb->truesize;
+
+	__skb_queue_tail(&msk->receive_queue, skb);
+	atomic64_set(&msk->rcv_wnd_sent, mptcp_subflow_get_mapped_dsn(subflow));
+	goto avoid_add_skb_to_sk;
+add_skb_to_sk:
 	__skb_queue_tail(&sk->sk_receive_queue, skb);
+avoid_add_skb_to_sk:
 	tp->syn_data_acked = 1;
 
 	/* u64_stats_update_begin(&tp->syncp) not needed here,
@@ -XXX,XX +XXX,XX @@ static struct sock *tcp_fastopen_create_child(struct sock *sk,
 
 	tp->rcv_nxt = TCP_SKB_CB(skb)->seq + 1;
 
-	tcp_fastopen_add_skb(child, skb);
+	tcp_fastopen_add_skb(child, skb, req);
 
 	tcp_rsk(req)->rcv_nxt = tp->rcv_nxt;
 	tp->rcv_wup = tp->rcv_nxt;
@@ -XXX,XX +XXX,XX @@ struct sock *tcp_try_fastopen(struct sock *sk, struct sk_buff *skb,
 	bool syn_data = TCP_SKB_CB(skb)->end_seq != TCP_SKB_CB(skb)->seq + 1;
 	int tcp_fastopen = READ_ONCE(sock_net(sk)->ipv4.sysctl_tcp_fastopen);
 	struct tcp_fastopen_cookie valid_foc = { .len = -1 };
+	struct tcp_sock *tp = tcp_sk(sk);
 	struct sock *child;
 	int ret = 0;
 
 	if (foc->len == 0) /* Client requests a cookie */
 		NET_INC_STATS(sock_net(sk), LINUX_MIB_TCPFASTOPENCOOKIEREQD);
 
-	if (!((tcp_fastopen & TFO_SERVER_ENABLE) &&
-	      (syn_data || foc->len >= 0) &&
-	      tcp_fastopen_queue_check(sk))) {
-		foc->len = -1;
-		return NULL;
+	if (tp->syn_fastopen && sk_is_mptcp(sk)) {
+		if (((syn_data || foc->len >= 0) &&
+		     tcp_fastopen_queue_check(sk))) {
+			foc->len = -1;
+			return NULL;
+		}
+	} else {
+		if (!((tcp_fastopen & TFO_SERVER_ENABLE) &&
+		      (syn_data || foc->len >= 0) &&
+		      tcp_fastopen_queue_check(sk))) {
+			foc->len = -1;
+			return NULL;
+		}
 	}
 
 	if (tcp_fastopen_no_cookie(sk, dst, TFO_SERVER_COOKIE_NOT_REQD))
diff --git a/net/ipv4/tcp_input.c b/net/ipv4/tcp_input.c
index XXXXXXX..XXXXXXX 100644
--- a/net/ipv4/tcp_input.c
+++ b/net/ipv4/tcp_input.c
@@ -XXX,XX +XXX,XX @@ static bool tcp_rcv_fastopen_synack(struct sock *sk, struct sk_buff *synack,
 			--tp->delivered;
 	}
 
-	tcp_fastopen_add_skb(sk, synack);
+	tcp_fastopen_add_skb(sk, synack, NULL);
 
 	return false;
 }
@@ -XXX,XX +XXX,XX @@ int tcp_conn_request(struct request_sock_ops *rsk_ops,
 	if (IS_ENABLED(CONFIG_SMC) && want_cookie)
 		tmp_opt.smc_ok = 0;
 
-	tmp_opt.tstamp_ok = tmp_opt.saw_tstamp;
+	if (foc.len == -1 && sk_is_mptcp(sk)) {
+		tmp_opt.tstamp_ok = tmp_opt.saw_tstamp;
+	} else {
+		tmp_opt.tstamp_ok = 0;
+		tcp_rsk(req)->ts_off = 1;
+		tp->syn_fastopen = 1;
+	}
+
 	tcp_openreq_init(req, &tmp_opt, skb, sk);
 	inet_rsk(req)->no_srccheck = inet_sk(sk)->transparent;
 
diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -XXX,XX +XXX,XX @@ static void __mptcp_rmem_reclaim(struct sock *sk, int amount)
 	__sk_mem_reduce_allocated(sk, amount);
 }
 
-static void mptcp_rmem_uncharge(struct sock *sk, int size)
+void mptcp_rmem_uncharge(struct sock *sk, int size)
 {
 	struct mptcp_sock *msk = mptcp_sk(sk);
 	int reclaimable;
@@ -XXX,XX +XXX,XX @@ static void mptcp_rmem_uncharge(struct sock *sk, int size)
 		__mptcp_rmem_reclaim(sk, reclaimable);
 }
 
-static void mptcp_rfree(struct sk_buff *skb)
+void mptcp_rfree(struct sk_buff *skb)
 {
 	unsigned int len = skb->truesize;
 	struct sock *sk = skb->sk;
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index XXXXXXX..XXXXXXX 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -XXX,XX +XXX,XX @@ void mptcp_event_addr_announced(const struct sock *ssk, const struct mptcp_addr_
 void mptcp_event_addr_removed(const struct mptcp_sock *msk, u8 id);
 bool mptcp_userspace_pm_active(const struct mptcp_sock *msk);
 int mptcp_stream_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags);
+void mptcp_rmem_uncharge(struct sock *sk, int size);
+void mptcp_rfree(struct sk_buff *skb);
 
 // Fast Open Mechanism functions begin
 int mptcp_setsockopt_sol_tcp_fastopen(struct mptcp_sock *msk, sockptr_t optval,
-- 
2.25.1

MPTFO tests: these are examples of initiator (sendto) and listener,
probably are going to be integrated to the mptcp_connect.* selftests