8 files changed, 252 insertions, 7 deletions

diff --git a/sys/netlink/netlink.h b/sys/netlink/netlink.h
index 3623ae754951..0021be4ea786 100644
--- a/sys/netlink/netlink.h
+++ b/sys/netlink/netlink.h
@@ -89,6 +89,7 @@ struct sockaddr_nl {
 #define NETLINK_EXT_ACK			11 /* Ack support for receiving additional TLVs in ack */
 #define NETLINK_GET_STRICT_CHK		12 /* Strict header checking */
 
+#define	NETLINK_MSG_INFO		257 /* (FreeBSD-specific) Receive message originator data in cmsg */
 
 /*
  * RFC 3549, 2.3.2 Netlink Message Header
@@ -183,6 +184,15 @@ enum nlmsgerr_attrs {
 	NLMSGERR_ATTR_MAX = __NLMSGERR_ATTR_MAX - 1
 };
 
+/* FreeBSD-specific debugging info */
+
+enum nlmsginfo_attrs {
+	NLMSGINFO_ATTR_UNUSED,
+	NLMSGINFO_ATTR_PROCESS_ID	= 1, /* u32, source process PID */
+	NLMSGINFO_ATTR_PORT_ID		= 2, /* u32, source socket nl_pid */
+	NLMSGINFO_ATTR_SEQ_ID		= 3, /* u32, source message seq_id */
+};
+
 
 #ifndef roundup2
 #define	roundup2(x, y)	(((x)+((y)-1))&(~((y)-1))) /* if y is powers of two */
diff --git a/sys/netlink/netlink_ctl.h b/sys/netlink/netlink_ctl.h
index 6c195c0217a9..9b51e9492a41 100644
--- a/sys/netlink/netlink_ctl.h
+++ b/sys/netlink/netlink_ctl.h
@@ -108,5 +108,22 @@ uint32_t genl_get_family_id(const struct genl_family *gf);
 typedef void (*genl_family_event_handler_t)(void *arg, const struct genl_family *gf, int action);
 EVENTHANDLER_DECLARE(genl_family_event, genl_family_event_handler_t);
 
+struct thread;
+#if defined(NETLINK) || defined(NETLINK_MODULE)
+/* Provide optimized calls to the functions inside the same linking unit */
+struct nlpcb *_nl_get_thread_nlp(struct thread *td);
+
+static inline struct nlpcb *
+nl_get_thread_nlp(struct thread *td)
+{
+	return (_nl_get_thread_nlp(td));
+}
+
+#else
+/* Provide access to the functions via netlink_glue.c */
+struct nlpcb *nl_get_thread_nlp(struct thread *td);
+
+#endif /* defined(NETLINK) || defined(NETLINK_MODULE) */
+
 #endif
 #endif
diff --git a/sys/netlink/netlink_domain.c b/sys/netlink/netlink_domain.c
index 8b0d09ac0b66..9cc2a5073fdd 100644
--- a/sys/netlink/netlink_domain.c
+++ b/sys/netlink/netlink_domain.c
@@ -38,6 +38,7 @@
 #include <sys/domain.h>
 #include <sys/jail.h>
 #include <sys/mbuf.h>
+#include <sys/osd.h>
 #include <sys/protosw.h>
 #include <sys/proc.h>
 #include <sys/ck.h>
@@ -84,6 +85,38 @@ SYSCTL_OID(_net_netlink, OID_AUTO, nl_maxsockbuf,
     sysctl_handle_nl_maxsockbuf, "LU",
     "Maximum Netlink socket buffer size");
 
+
+static unsigned int osd_slot_id = 0;
+
+void
+nl_osd_register(void)
+{
+	osd_slot_id = osd_register(OSD_THREAD, NULL, NULL);
+}
+
+void
+nl_osd_unregister(void)
+{
+	osd_deregister(OSD_THREAD, osd_slot_id);
+}
+
+struct nlpcb *
+_nl_get_thread_nlp(struct thread *td)
+{
+	return (osd_get(OSD_THREAD, &td->td_osd, osd_slot_id));
+}
+
+void
+nl_set_thread_nlp(struct thread *td, struct nlpcb *nlp)
+{
+	NLP_LOG(LOG_DEBUG2, nlp, "Set thread %p nlp to %p (slot %u)", td, nlp, osd_slot_id);
+	if (osd_set(OSD_THREAD, &td->td_osd, osd_slot_id, nlp) == 0)
+		return;
+	/* Failed, need to realloc */
+	void **rsv = osd_reserve(osd_slot_id);
+	osd_set_reserved(OSD_THREAD, &td->td_osd, osd_slot_id, rsv, nlp);
+}
+
 /*
  * Looks up a nlpcb struct based on the @portid. Need to claim nlsock_mtx.
  * Returns nlpcb pointer if present else NULL
@@ -144,6 +177,15 @@ nl_get_groups_compat(struct nlpcb *nlp)
 	return (groups_mask);
 }
 
+static void
+nl_send_one_group(struct mbuf *m, struct nlpcb *nlp, int num_messages,
+    int io_flags)
+{
+	if (__predict_false(nlp->nl_flags & NLF_MSG_INFO))
+		nl_add_msg_info(m);
+	nl_send_one(m, nlp, num_messages, io_flags);
+}
+
 /*
  * Broadcasts message @m to the protocol @proto group specified by @group_id
  */
@@ -180,7 +222,8 @@ nl_send_group(struct mbuf *m, int num_messages, int proto, int group_id)
 				struct mbuf *m_copy;
 				m_copy = m_copym(m, 0, M_COPYALL, M_NOWAIT);
 				if (m_copy != NULL)
-					nl_send_one(m_copy, nlp_last, num_messages, io_flags);
+					nl_send_one_group(m_copy, nlp_last,
+					    num_messages, io_flags);
 				else {
 					NLP_LOCK(nlp_last);
 					if (nlp_last->nl_socket != NULL)
@@ -192,7 +235,7 @@ nl_send_group(struct mbuf *m, int num_messages, int proto, int group_id)
 		}
 	}
 	if (nlp_last != NULL)
-		nl_send_one(m, nlp_last, num_messages, io_flags);
+		nl_send_one_group(m, nlp_last, num_messages, io_flags);
 	else
 		m_freem(m);
 
@@ -296,6 +339,7 @@ nl_pru_attach(struct socket *so, int proto, struct thread *td)
 	nlp->nl_linux = is_linux;
 	nlp->nl_active = true;
 	nlp->nl_unconstrained_vnet = !jailed_without_vnet(so->so_cred);
+	nlp->nl_need_thread_setup = true;
 	NLP_LOCK_INIT(nlp);
 	refcount_init(&nlp->nl_refcount, 1);
 	nl_init_io(nlp);
@@ -589,6 +633,8 @@ nl_getoptflag(int sopt_name)
 		return (NLF_EXT_ACK);
 	case NETLINK_GET_STRICT_CHK:
 		return (NLF_STRICT);
+	case NETLINK_MSG_INFO:
+		return (NLF_MSG_INFO);
 	}
 
 	return (0);
@@ -630,12 +676,18 @@ nl_ctloutput(struct socket *so, struct sockopt *sopt)
 		case NETLINK_CAP_ACK:
 		case NETLINK_EXT_ACK:
 		case NETLINK_GET_STRICT_CHK:
+		case NETLINK_MSG_INFO:
 			error = sooptcopyin(sopt, &optval, sizeof(optval), sizeof(optval));
 			if (error != 0)
 				break;
 
 			flag = nl_getoptflag(sopt->sopt_name);
 
+			if ((flag == NLF_MSG_INFO) && nlp->nl_linux) {
+				error = EINVAL;
+				break;
+			}
+
 			NLCTL_WLOCK(ctl);
 			if (optval != 0)
 				nlp->nl_flags |= flag;
@@ -658,6 +710,7 @@ nl_ctloutput(struct socket *so, struct sockopt *sopt)
 		case NETLINK_CAP_ACK:
 		case NETLINK_EXT_ACK:
 		case NETLINK_GET_STRICT_CHK:
+		case NETLINK_MSG_INFO:
 			NLCTL_RLOCK(ctl);
 			optval = (nlp->nl_flags & nl_getoptflag(sopt->sopt_name)) != 0;
 			NLCTL_RUNLOCK(ctl);
diff --git a/sys/netlink/netlink_glue.c b/sys/netlink/netlink_glue.c
index 940ff2de4859..e881bf019f52 100644
--- a/sys/netlink/netlink_glue.c
+++ b/sys/netlink/netlink_glue.c
@@ -191,6 +191,12 @@ nl_store_ifp_cookie_stub(struct nl_pstate *npt __unused, struct ifnet *ifp __unu
 {
 }
 
+static struct nlpcb *
+nl_get_thread_nlp_stub(struct thread *td __unused)
+{
+	return (NULL);
+}
+
 const static struct nl_function_wrapper nl_stub = {
 	.nlmsg_add = nlmsg_add_stub,
 	.nlmsg_refill_buffer = nlmsg_refill_buffer_stub,
@@ -204,6 +210,7 @@ const static struct nl_function_wrapper nl_stub = {
 	.nlmsg_end_dump = nlmsg_end_dump_stub,
 	.nl_modify_ifp_generic = nl_modify_ifp_generic_stub,
 	.nl_store_ifp_cookie = nl_store_ifp_cookie_stub,
+	.nl_get_thread_nlp = nl_get_thread_nlp_stub,
 };
 
 /*
@@ -292,5 +299,11 @@ nl_store_ifp_cookie(struct nl_pstate *npt, struct ifnet *ifp)
 	return (_nl->nl_store_ifp_cookie(npt, ifp));
 }
 
+struct nlpcb *
+nl_get_thread_nlp(struct thread *td)
+{
+	return (_nl->nl_get_thread_nlp(td));
+}
+
 #endif /* !NETLINK */
 
diff --git a/sys/netlink/netlink_io.c b/sys/netlink/netlink_io.c
index b40ffaab7dd9..db0a97eef0fd 100644
--- a/sys/netlink/netlink_io.c
+++ b/sys/netlink/netlink_io.c
@@ -125,6 +125,55 @@ queue_free(struct nl_io_queue *q)
 	q->length = 0;
 }
 
+void
+nl_add_msg_info(struct mbuf *m)
+{
+	struct nlpcb *nlp = nl_get_thread_nlp(curthread);
+	NL_LOG(LOG_DEBUG2, "Trying to recover nlp from thread %p: %p",
+	    curthread, nlp);
+
+	if (nlp == NULL)
+		return;
+
+	/* Prepare what we want to encode - PID, socket PID & msg seq */
+	struct {
+		struct nlattr nla;
+		uint32_t val;
+	} data[] = {
+		{
+			.nla.nla_len = sizeof(struct nlattr) + sizeof(uint32_t),
+			.nla.nla_type = NLMSGINFO_ATTR_PROCESS_ID,
+			.val = nlp->nl_process_id,
+		},
+		{
+			.nla.nla_len = sizeof(struct nlattr) + sizeof(uint32_t),
+			.nla.nla_type = NLMSGINFO_ATTR_PORT_ID,
+			.val = nlp->nl_port,
+		},
+	};
+
+
+	while (m->m_next != NULL)
+		m = m->m_next;
+	m->m_next = sbcreatecontrol(data, sizeof(data),
+	    NETLINK_MSG_INFO, SOL_NETLINK, M_NOWAIT);
+
+	NL_LOG(LOG_DEBUG2, "Storing %lu bytes of data, ctl: %p", sizeof(data), m->m_next);
+}
+
+static __noinline struct mbuf *
+extract_msg_info(struct mbuf *m)
+{
+	while (m->m_next != NULL) {
+		if (m->m_next->m_type == MT_CONTROL) {
+			struct mbuf *ctl = m->m_next;
+			m->m_next = NULL;
+			return (ctl);
+		}
+		m = m->m_next;
+	}
+	return (NULL);
+}
 
 static void
 nl_schedule_taskqueue(struct nlpcb *nlp)
@@ -181,10 +230,16 @@ tx_check_locked(struct nlpcb *nlp)
 
 	while (true) {
 		struct mbuf *m = queue_head(&nlp->tx_queue);
-		if (m && sbappendaddr_locked(sb, nl_empty_src, m, NULL) != 0) {
-			/* appended successfully */
-			queue_pop(&nlp->tx_queue);
-			appended = true;
+		if (m != NULL) {
+			struct mbuf *ctl = NULL;
+			if (__predict_false(m->m_next != NULL))
+				ctl = extract_msg_info(m);
+			if (sbappendaddr_locked(sb, nl_empty_src, m, ctl) != 0) {
+				/* appended successfully */
+				queue_pop(&nlp->tx_queue);
+				appended = true;
+			} else
+				break;
 		} else
 			break;
 	}
@@ -257,6 +312,13 @@ nl_process_received(struct nlpcb *nlp)
 {
 	NL_LOG(LOG_DEBUG3, "taskqueue called");
 
+	if (__predict_false(nlp->nl_need_thread_setup)) {
+		nl_set_thread_nlp(curthread, nlp);
+		NLP_LOCK(nlp);
+		nlp->nl_need_thread_setup = false;
+		NLP_UNLOCK(nlp);
+	}
+
 	while (nl_process_received_one(nlp))
 		;
 }
@@ -374,7 +436,10 @@ nl_send_one(struct mbuf *m, struct nlpcb *nlp, int num_messages, int io_flags)
 	}
 
 	struct socket *so = nlp->nl_socket;
-	if (sbappendaddr(&so->so_rcv, nl_empty_src, m, NULL) != 0) {
+	struct mbuf *ctl = NULL;
+	if (__predict_false(m->m_next != NULL))
+		ctl = extract_msg_info(m);
+	if (sbappendaddr(&so->so_rcv, nl_empty_src, m, ctl) != 0) {
 		sorwakeup(so);
 		NLP_LOG(LOG_DEBUG3, nlp, "appended data & woken up");
 	} else {
diff --git a/sys/netlink/netlink_module.c b/sys/netlink/netlink_module.c
index 31faf1d003d9..6835c4a0e730 100644
--- a/sys/netlink/netlink_module.c
+++ b/sys/netlink/netlink_module.c
@@ -189,6 +189,7 @@ const static struct nl_function_wrapper nl_module = {
 	.nlmsg_end_dump = _nlmsg_end_dump,
 	.nl_modify_ifp_generic = _nl_modify_ifp_generic,
 	.nl_store_ifp_cookie = _nl_store_ifp_cookie,
+	.nl_get_thread_nlp = _nl_get_thread_nlp,
 };
 #endif
 
@@ -222,6 +223,7 @@ netlink_modevent(module_t mod __unused, int what, void *priv __unused)
 	switch (what) {
 	case MOD_LOAD:
 		NL_LOG(LOG_DEBUG2, "Loading");
+		nl_osd_register();
 #if !defined(NETLINK) && defined(NETLINK_MODULE)
 		nl_set_functions(&nl_module);
 #endif
@@ -235,6 +237,7 @@ netlink_modevent(module_t mod __unused, int what, void *priv __unused)
 #if !defined(NETLINK) && defined(NETLINK_MODULE)
 			nl_set_functions(NULL);
 #endif
+			nl_osd_unregister();
 		} else
 			ret = EBUSY;
 		break;
diff --git a/sys/netlink/netlink_snl.h b/sys/netlink/netlink_snl.h
index 191a303111fa..4cb1b3e13abc 100644
--- a/sys/netlink/netlink_snl.h
+++ b/sys/netlink/netlink_snl.h
@@ -277,6 +277,55 @@ snl_get_seq(struct snl_state *ss)
 	return (++ss->seq);
 }
 
+struct snl_msg_info {
+	int		cmsg_type;
+	int		cmsg_level;
+	uint32_t	process_id;
+	uint8_t		port_id;
+	uint8_t		seq_id;
+};
+static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg,
+    struct snl_msg_info *attrs);
+
+static inline struct nlmsghdr *
+snl_read_message_dbg(struct snl_state *ss, struct snl_msg_info *cinfo)
+{
+	memset(cinfo, 0, sizeof(*cinfo));
+
+	if (ss->off == ss->datalen) {
+		struct sockaddr_nl nladdr;
+		char cbuf[64];
+
+		struct iovec iov = {
+			.iov_base = ss->buf,
+			.iov_len = ss->bufsize,
+		};
+		struct msghdr msg = {
+			.msg_name = &nladdr,
+			.msg_namelen = sizeof(nladdr),
+			.msg_iov = &iov,
+			.msg_iovlen = 1,
+			.msg_control = cbuf,
+			.msg_controllen = sizeof(cbuf),
+		};
+		ss->off = 0;
+		ss->datalen = 0;
+		for (;;) {
+			ssize_t datalen = recvmsg(ss->fd, &msg, 0);
+			if (datalen > 0) {
+				ss->datalen = datalen;
+				parse_cmsg(ss, &msg, cinfo);
+				break;
+			} else if (errno != EINTR)
+				return (NULL);
+		}
+	}
+	struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off];
+	ss->off += NLMSG_ALIGN(hdr->nlmsg_len);
+	return (hdr);
+}
+
+
 static inline struct nlmsghdr *
 snl_read_message(struct snl_state *ss)
 {
@@ -661,6 +710,33 @@ snl_read_reply_code(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_
 	return (false);
 }
 
+#define	_OUT(_field)	offsetof(struct snl_msg_info, _field)
+static const struct snl_attr_parser _nla_p_cinfo[] = {
+	{ .type = NLMSGINFO_ATTR_PROCESS_ID, .off = _OUT(process_id), .cb = snl_attr_get_uint32 },
+	{ .type = NLMSGINFO_ATTR_PORT_ID, .off = _OUT(port_id), .cb = snl_attr_get_uint32 },
+	{ .type = NLMSGINFO_ATTR_SEQ_ID, .off = _OUT(seq_id), .cb = snl_attr_get_uint32 },
+};
+#undef _OUT
+SNL_DECLARE_ATTR_PARSER(snl_msg_info_parser, _nla_p_cinfo);
+
+static inline bool
+parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs)
+{
+	for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
+	    cmsg = CMSG_NXTHDR(msg, cmsg)) {
+		if (cmsg->cmsg_level != SOL_NETLINK || cmsg->cmsg_type != NETLINK_MSG_INFO)
+			continue;
+
+		void *data = CMSG_DATA(cmsg);
+		int len = cmsg->cmsg_len - ((char *)data - (char *)cmsg);
+		const struct snl_hdr_parser *ps = &snl_msg_info_parser;
+
+		return (snl_parse_attrs_raw(ss, data, len, ps->np, ps->np_size, attrs));
+	}
+
+	return (false);
+}
+
 /*
  * Assumes e is zeroed
  */
diff --git a/sys/netlink/netlink_var.h b/sys/netlink/netlink_var.h
index cb1e3974b5f5..7882bfbf5359 100644
--- a/sys/netlink/netlink_var.h
+++ b/sys/netlink/netlink_var.h
@@ -62,6 +62,7 @@ struct nlpcb {
 	bool			nl_tx_blocked; /* No new requests accepted */
 	bool			nl_linux; /* true if running under compat */
 	bool			nl_unconstrained_vnet; /* true if running under VNET jail (or without jail) */
+	bool			nl_need_thread_setup;
 	struct nl_io_queue	rx_queue;
 	struct nl_io_queue	tx_queue;
 	struct taskqueue	*nl_taskqueue;
@@ -88,6 +89,7 @@ struct nlpcb {
 #define NLF_CAP_ACK             0x01 /* Do not send message body with errmsg */
 #define NLF_EXT_ACK             0x02 /* Allow including extended TLVs in ack */
 #define	NLF_STRICT		0x04 /* Perform strict header checks */
+#define	NLF_MSG_INFO		0x08 /* Send caller info along with the notifications */
 
 SYSCTL_DECL(_net_netlink);
 SYSCTL_DECL(_net_netlink_debug);
@@ -130,6 +132,9 @@ extern struct nl_proto_handler *nl_handlers;
 
 /* netlink_domain.c */
 void nl_send_group(struct mbuf *m, int cnt, int proto, int group_id);
+void nl_osd_register(void);
+void nl_osd_unregister(void);
+void nl_set_thread_nlp(struct thread *td, struct nlpcb *nlp);
 
 /* netlink_io.c */
 #define	NL_IOF_UNTRANSLATED	0x01
@@ -144,6 +149,8 @@ void nl_free_io(struct nlpcb *nlp);
 void nl_taskqueue_handler(void *_arg, int pending);
 int nl_receive_async(struct mbuf *m, struct socket *so);
 void nl_process_receive_locked(struct nlpcb *nlp);
+void nl_set_source_metadata(struct mbuf *m, int num_messages);
+void nl_add_msg_info(struct mbuf *m);
 
 /* netlink_generic.c */
 struct genl_family {
@@ -193,6 +200,7 @@ struct nl_function_wrapper {
 	int (*nl_modify_ifp_generic)(struct ifnet *ifp, struct nl_parsed_link *lattrs,
 	    const struct nlattr_bmask *bm, struct nl_pstate *npt);
 	void (*nl_store_ifp_cookie)(struct nl_pstate *npt, struct ifnet *ifp);
+	struct nlpcb * (*nl_get_thread_nlp)(struct  thread *td);
 };
 void nl_set_functions(const struct nl_function_wrapper *nl);