[net-next,v3,02/17] mptcp: Add path manager interface

diff --git a/net/mptcp/Makefile b/net/mptcp/Makefile
index 4e98d9edfd0a..2848d723c252 100644
--- a/net/mptcp/Makefile
+++ b/net/mptcp/Makefile
@@ -1,4 +1,4 @@ 
 # SPDX-License-Identifier: GPL-2.0
 obj-$(CONFIG_MPTCP) += mptcp.o
 
-mptcp-y := protocol.o subflow.o options.o token.o crypto.o ctrl.o
+mptcp-y := protocol.o subflow.o options.o token.o crypto.o ctrl.o pm.o
diff --git a/net/mptcp/options.c b/net/mptcp/options.c
index 6c6c18a09a40..a3661318a7af 100644
--- a/net/mptcp/options.c
+++ b/net/mptcp/options.c
@@ -492,36 +492,35 @@  static bool mptcp_established_options_addr(struct sock *sk,
 {
 	struct mptcp_subflow_context *subflow = mptcp_subflow_ctx(sk);
 	struct mptcp_sock *msk = mptcp_sk(subflow->conn);
-	struct sockaddr_storage saddr;
-	u8 id;
+	struct mptcp_addr_info saddr;
+	int len;
 
-	id = 0;
-	memset(&saddr, 0, sizeof(saddr));
+	if (!mptcp_pm_should_signal(msk) ||
+	    !(mptcp_pm_addr_signal(msk, remaining, &saddr)))
+		return false;
+
+	len = mptcp_add_addr_len(saddr.family);
+	if (remaining < len)
+		return false;
 
-	if (saddr.ss_family == AF_INET) {
-		if (remaining < TCPOLEN_MPTCP_ADD_ADDR)
-			return false;
+	*size = len;
+	opts->addr_id = saddr.id;
+	if (saddr.family == AF_INET) {
 		opts->suboptions |= OPTION_MPTCP_ADD_ADDR;
-		opts->addr_id = id;
-		opts->addr = ((struct sockaddr_in *)&saddr)->sin_addr;
+		opts->addr = saddr.addr;
 		opts->ahmac = add_addr_generate_hmac(msk->local_key,
 						     msk->remote_key,
 						     opts->addr_id,
 						     &opts->addr);
-		*size = TCPOLEN_MPTCP_ADD_ADDR;
 	}
 #if IS_ENABLED(CONFIG_MPTCP_IPV6)
-	else if (saddr.ss_family == AF_INET6) {
-		if (remaining < TCPOLEN_MPTCP_ADD_ADDR6)
-			return false;
+	else if (saddr.family == AF_INET6) {
 		opts->suboptions |= OPTION_MPTCP_ADD_ADDR6;
-		opts->addr_id = id;
+		opts->addr6 = saddr.addr6;
 		opts->ahmac = add_addr6_generate_hmac(msk->local_key,
 						      msk->remote_key,
 						      opts->addr_id,
 						      &opts->addr6);
-		opts->addr6 = ((struct sockaddr_in6 *)&saddr)->sin6_addr;
-		*size = TCPOLEN_MPTCP_ADD_ADDR6;
 	}
 #endif
 	pr_debug("addr_id=%d, ahmac=%llu", opts->addr_id, opts->ahmac);
@@ -607,10 +606,37 @@  static bool check_fully_established(struct mptcp_subflow_context *subflow,
 	return true;
 }
 
+static bool add_addr_hmac_valid(struct mptcp_sock *msk,
+				struct mptcp_options_received *mp_opt)
+{
+	u64 hmac = 0;
+
+	if (mp_opt->echo)
+		return true;
+
+	if (mp_opt->family == MPTCP_ADDR_IPVERSION_4)
+		hmac = add_addr_generate_hmac(msk->remote_key,
+					      msk->local_key,
+					      mp_opt->addr_id, &mp_opt->addr);
+#if IS_ENABLED(CONFIG_MPTCP_IPV6)
+	else
+		hmac = add_addr6_generate_hmac(msk->remote_key,
+					       msk->local_key,
+					       mp_opt->addr_id, &mp_opt->addr6);
+#endif
+
+	pr_debug("msk=%p, ahmac=%llu, mp_opt->ahmac=%llu\n",
+		 msk, (unsigned long long)hmac,
+		 (unsigned long long)mp_opt->ahmac);
+
+	return hmac == mp_opt->ahmac;
+}
+
 void mptcp_incoming_options(struct sock *sk, struct sk_buff *skb,
 			    struct tcp_options_received *opt_rx)
 {
 	struct mptcp_subflow_context *subflow = mptcp_subflow_ctx(sk);
+	struct mptcp_sock *msk = mptcp_sk(subflow->conn);
 	struct mptcp_options_received *mp_opt;
 	struct mptcp_ext *mpext;
 
@@ -618,6 +644,26 @@  void mptcp_incoming_options(struct sock *sk, struct sk_buff *skb,
 	if (!check_fully_established(subflow, skb, mp_opt))
 		return;
 
+	if (mp_opt->add_addr && add_addr_hmac_valid(msk, mp_opt)) {
+		struct mptcp_addr_info addr;
+
+		addr.port = htons(mp_opt->port);
+		addr.id = mp_opt->addr_id;
+		if (mp_opt->family == MPTCP_ADDR_IPVERSION_4) {
+			addr.family = AF_INET;
+			addr.addr = mp_opt->addr;
+		}
+#if IS_ENABLED(CONFIG_MPTCP_IPV6)
+		else if (mp_opt->family == MPTCP_ADDR_IPVERSION_6) {
+			addr.family = AF_INET6;
+			addr.addr6 = mp_opt->addr6;
+		}
+#endif
+		if (!mp_opt->echo)
+			mptcp_pm_add_addr_received(msk, &addr);
+		mp_opt->add_addr = 0;
+	}
+
 	if (!mp_opt->dss)
 		return;
 
@@ -654,6 +700,8 @@  void mptcp_incoming_options(struct sock *sk, struct sk_buff *skb,
 	}
 
 	mpext->data_fin = mp_opt->data_fin;
+
+	mptcp_pm_fully_established(msk);
 }
 
 void mptcp_write_options(__be32 *ptr, struct mptcp_out_options *opts)
diff --git a/net/mptcp/pm.c b/net/mptcp/pm.c
new file mode 100644
index 000000000000..ad837da0193d
--- /dev/null
+++ b/net/mptcp/pm.c
@@ -0,0 +1,113 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/* Multipath TCP
+ *
+ * Copyright (c) 2019, Intel Corporation.
+ */
+#include <linux/kernel.h>
+#include <net/tcp.h>
+#include <net/mptcp.h>
+#include "protocol.h"
+
+static struct workqueue_struct *pm_wq;
+
+/* path manager command handlers */
+
+int mptcp_pm_announce_addr(struct mptcp_sock *msk,
+			   const struct mptcp_addr_info *addr)
+{
+	return -ENOTSUPP;
+}
+
+int mptcp_pm_remove_addr(struct mptcp_sock *msk, u8 local_id)
+{
+	return -ENOTSUPP;
+}
+
+int mptcp_pm_remove_subflow(struct mptcp_sock *msk, u8 remote_id)
+{
+	return -ENOTSUPP;
+}
+
+/* path manager event handlers */
+
+void mptcp_pm_new_connection(struct mptcp_sock *msk, int server_side)
+{
+	struct mptcp_pm_data *pm = &msk->pm;
+
+	pr_debug("msk=%p, token=%u side=%d", msk, msk->token, server_side);
+
+	WRITE_ONCE(pm->server_side, server_side);
+}
+
+bool mptcp_pm_allow_new_subflow(struct mptcp_sock *msk)
+{
+	pr_debug("msk=%p", msk);
+	return false;
+}
+
+void mptcp_pm_fully_established(struct mptcp_sock *msk)
+{
+	pr_debug("msk=%p", msk);
+}
+
+void mptcp_pm_connection_closed(struct mptcp_sock *msk)
+{
+	pr_debug("msk=%p", msk);
+}
+
+void mptcp_pm_subflow_established(struct mptcp_sock *msk,
+				  struct mptcp_subflow_context *subflow)
+{
+	pr_debug("msk=%p", msk);
+}
+
+void mptcp_pm_subflow_closed(struct mptcp_sock *msk, u8 id)
+{
+	pr_debug("msk=%p", msk);
+}
+
+void mptcp_pm_add_addr_received(struct mptcp_sock *msk,
+				const struct mptcp_addr_info *addr)
+{
+	pr_debug("msk=%p, remote_id=%d", msk, addr->id);
+}
+
+/* path manager helpers */
+
+bool mptcp_pm_addr_signal(struct mptcp_sock *msk, unsigned int remaining,
+			  struct mptcp_addr_info *saddr)
+{
+	return false;
+}
+
+int mptcp_pm_get_local_id(struct mptcp_sock *msk, struct sock_common *skc)
+{
+	return 0;
+}
+
+static void pm_worker(struct work_struct *work)
+{
+}
+
+void mptcp_pm_data_init(struct mptcp_sock *msk)
+{
+	msk->pm.add_addr_signaled = 0;
+	msk->pm.add_addr_accepted = 0;
+	msk->pm.local_addr_used = 0;
+	msk->pm.subflows = 0;
+	WRITE_ONCE(msk->pm.work_pending, false);
+	WRITE_ONCE(msk->pm.addr_signal, false);
+	WRITE_ONCE(msk->pm.accept_addr, false);
+	WRITE_ONCE(msk->pm.accept_subflow, false);
+	msk->pm.status = 0;
+
+	spin_lock_init(&msk->pm.lock);
+	INIT_WORK(&msk->pm.work, pm_worker);
+}
+
+void mptcp_pm_init(void)
+{
+	pm_wq = alloc_workqueue("pm_wq", WQ_UNBOUND | WQ_MEM_RECLAIM, 8);
+	if (!pm_wq)
+		panic("Failed to allocate workqueue");
+}
diff --git a/net/mptcp/protocol.c b/net/mptcp/protocol.c
index 92d5382e71f4..5f93e8c94ef9 100644
--- a/net/mptcp/protocol.c
+++ b/net/mptcp/protocol.c
@@ -703,6 +703,8 @@  static int __mptcp_init_sock(struct sock *sk)
 	msk->first = NULL;
 	inet_csk(sk)->icsk_sync_mss = mptcp_sync_mss;
 
+	mptcp_pm_data_init(msk);
+
 	return 0;
 }
 
@@ -1055,6 +1057,8 @@  void mptcp_finish_connect(struct sock *ssk)
 	WRITE_ONCE(msk->write_seq, subflow->idsn + 1);
 	WRITE_ONCE(msk->ack_seq, ack_seq);
 	WRITE_ONCE(msk->can_ack, 1);
+
+	mptcp_pm_new_connection(msk, 0);
 }
 
 static void mptcp_sock_graft(struct sock *sk, struct socket *parent)
@@ -1377,6 +1381,7 @@  void mptcp_proto_init(void)
 	mptcp_prot.h.hashinfo = tcp_prot.h.hashinfo;
 
 	mptcp_subflow_init();
+	mptcp_pm_init();
 
 	if (proto_register(&mptcp_prot, 1) != 0)
 		panic("Failed to register MPTCP proto.\n");
diff --git a/net/mptcp/protocol.h b/net/mptcp/protocol.h
index 471e013d1c32..8d4761ae3951 100644
--- a/net/mptcp/protocol.h
+++ b/net/mptcp/protocol.h
@@ -84,6 +84,50 @@  static inline __be32 mptcp_option(u8 subopt, u8 len, u8 nib, u8 field)
 		     ((nib & 0xF) << 8) | field);
 }
 
+#define MPTCP_PM_MAX_ADDR	4
+
+struct mptcp_addr_info {
+	sa_family_t		family;
+	__be16			port;
+	u8			id;
+	union {
+		struct in_addr addr;
+#if IS_ENABLED(CONFIG_MPTCP_IPV6)
+		struct in6_addr addr6;
+#endif
+	};
+};
+
+enum mptcp_pm_status {
+	MPTCP_PM_ADD_ADDR_RECEIVED,
+	MPTCP_PM_ESTABLISHED,
+	MPTCP_PM_SUBFLOW_ESTABLISHED,
+};
+
+struct mptcp_pm_data {
+	struct mptcp_addr_info local;
+	struct mptcp_addr_info remote;
+
+	spinlock_t	lock;		/*protects the whole PM data */
+
+	bool		addr_signal;
+	bool		server_side;
+	bool		work_pending;
+	bool		accept_addr;
+	bool		accept_subflow;
+	u8		add_addr_signaled;
+	u8		add_addr_accepted;
+	u8		local_addr_used;
+	u8		subflows;
+	u8		add_addr_signal_max;
+	u8		add_addr_accept_max;
+	u8		local_addr_max;
+	u8		subflows_max;
+	u8		status;
+
+	struct		work_struct work;
+};
+
 /* MPTCP connection sock */
 struct mptcp_sock {
 	/* inet_connection_sock must be the first member */
@@ -100,6 +144,7 @@  struct mptcp_sock {
 	struct skb_ext	*cached_ext;	/* for the next sendmsg */
 	struct socket	*subflow; /* outgoing connect/listener/!mp_capable */
 	struct sock	*first;
+	struct mptcp_pm_data	pm;
 };
 
 #define mptcp_for_each_subflow(__msk, __subflow)			\
@@ -116,6 +161,7 @@  struct mptcp_subflow_request_sock {
 		mp_join : 1,
 		backup : 1,
 		remote_key_valid : 1;
+	u8	local_id;
 	u64	local_key;
 	u64	remote_key;
 	u64	idsn;
@@ -246,6 +292,39 @@  static inline void mptcp_crypto_key_gen_sha(u64 *key, u32 *token, u64 *idsn)
 
 void mptcp_crypto_hmac_sha(u64 key1, u64 key2, u8 *msg, int len, void *hmac);
 
+void mptcp_pm_init(void);
+void mptcp_pm_data_init(struct mptcp_sock *msk);
+void mptcp_pm_new_connection(struct mptcp_sock *msk, int server_side);
+void mptcp_pm_fully_established(struct mptcp_sock *msk);
+bool mptcp_pm_allow_new_subflow(struct mptcp_sock *msk);
+void mptcp_pm_connection_closed(struct mptcp_sock *msk);
+void mptcp_pm_subflow_established(struct mptcp_sock *msk,
+				  struct mptcp_subflow_context *subflow);
+void mptcp_pm_subflow_closed(struct mptcp_sock *msk, u8 id);
+void mptcp_pm_add_addr_received(struct mptcp_sock *msk,
+				const struct mptcp_addr_info *addr);
+
+int mptcp_pm_announce_addr(struct mptcp_sock *msk,
+			   const struct mptcp_addr_info *addr);
+int mptcp_pm_remove_addr(struct mptcp_sock *msk, u8 local_id);
+int mptcp_pm_remove_subflow(struct mptcp_sock *msk, u8 remote_id);
+
+static inline bool mptcp_pm_should_signal(struct mptcp_sock *msk)
+{
+	return READ_ONCE(msk->pm.addr_signal);
+}
+
+static inline unsigned int mptcp_add_addr_len(int family)
+{
+	if (family == AF_INET)
+		return TCPOLEN_MPTCP_ADD_ADDR;
+	return TCPOLEN_MPTCP_ADD_ADDR6;
+}
+
+bool mptcp_pm_addr_signal(struct mptcp_sock *msk, unsigned int remaining,
+			  struct mptcp_addr_info *saddr);
+int mptcp_pm_get_local_id(struct mptcp_sock *msk, struct sock_common *skc);
+
 static inline struct mptcp_ext *mptcp_get_ext(struct sk_buff *skb)
 {
 	return (struct mptcp_ext *)skb_ext_find(skb, SKB_EXT_MPTCP);
diff --git a/net/mptcp/subflow.c b/net/mptcp/subflow.c
index 5bae12da2769..eb82cb33dde2 100644
--- a/net/mptcp/subflow.c
+++ b/net/mptcp/subflow.c
@@ -240,8 +240,8 @@  static struct sock *subflow_syn_recv_sock(const struct sock *sk,
 			/* new mpc subflow takes ownership of the newly
 			 * created mptcp socket
 			 */
-			inet_sk_state_store((struct sock *)new_msk,
-					    TCP_ESTABLISHED);
+			inet_sk_state_store(new_msk, TCP_ESTABLISHED);
+			mptcp_pm_new_connection(mptcp_sk(new_msk), 1);
 			ctx->conn = new_msk;
 			new_msk = NULL;
 		}