2 files changed, 123 insertions, 2 deletions

diff --git a/include/linux/sock_diag.h b/include/linux/sock_diag.h
new file mode 100644
index 000000000000..ba4933b1213b
--- /dev/null
+++ b/include/linux/sock_diag.h
@@ -0,0 +1,23 @@
+#ifndef __SOCK_DIAG_H__
+#define __SOCK_DIAG_H__
+struct sk_buff;
+struct nlmsghdr;
+
+struct sock_diag_req {
+        __u8    sdiag_family;
+        __u8    sdiag_protocol;
+};
+
+struct sock_diag_handler {
+        __u8 family;
+        int (*dump)(struct sk_buff *skb, struct nlmsghdr *nlh);
+};
+
+int sock_diag_register(struct sock_diag_handler *h);
+void sock_diag_unregister(struct sock_diag_handler *h);
+
+void sock_diag_register_inet_compat(int (*fn)(struct sk_buff *skb, struct nlmsghdr *nlh));
+void sock_diag_unregister_inet_compat(int (*fn)(struct sk_buff *skb, struct nlmsghdr *nlh));
+
+extern struct sock *sock_diag_nlsk;
+#endif
diff --git a/net/ipv4/inet_diag.c b/net/ipv4/inet_diag.c
index 58caecc343b1..877875ea3d71 100644
--- a/net/ipv4/inet_diag.c
+++ b/net/ipv4/inet_diag.c
@@ -33,6 +33,7 @@
 #include <linux/stddef.h>
 
 #include <linux/inet_diag.h>
+#include <linux/sock_diag.h>
 
 static const struct inet_diag_handler **inet_diag_table;
 
@@ -887,9 +888,91 @@ static int inet_diag_rcv_msg_compat(struct sk_buff *skb, struct nlmsghdr *nlh)
         return inet_diag_get_exact(skb, nlh);
 }
 
+static int inet_diag_handler_dump(struct sk_buff *skb, struct nlmsghdr *h)
+{
+        int hdrlen = sizeof(struct inet_diag_req);
+
+        if (nlmsg_len(h) < hdrlen)
+                return -EINVAL;
+
+        if (h->nlmsg_flags & NLM_F_DUMP) {
+                return -EAFNOSUPPORT;
+        }
+
+        return -EAFNOSUPPORT;
+}
+
+static struct sock_diag_handler inet_diag_handler = {
+        .family = AF_INET,
+        .dump = inet_diag_handler_dump,
+};
+
+static struct sock_diag_handler inet6_diag_handler = {
+        .family = AF_INET6,
+        .dump = inet_diag_handler_dump,
+};
+
+static struct sock_diag_handler *sock_diag_handlers[AF_MAX];
+static DEFINE_MUTEX(sock_diag_table_mutex);
+
+int sock_diag_register(struct sock_diag_handler *hndl)
+{
+        int err = 0;
+
+        if (hndl->family > AF_MAX)
+                return -EINVAL;
+
+        mutex_lock(&sock_diag_table_mutex);
+        if (sock_diag_handlers[hndl->family])
+                err = -EBUSY;
+        else
+                sock_diag_handlers[hndl->family] = hndl;
+        mutex_unlock(&sock_diag_table_mutex);
+
+        return err;
+}
+
+void sock_diag_unregister(struct sock_diag_handler *hnld)
+{
+        int family = hnld->family;
+
+        if (family > AF_MAX)
+                return;
+
+        mutex_lock(&sock_diag_table_mutex);
+        BUG_ON(sock_diag_handlers[family] != hnld);
+        sock_diag_handlers[family] = NULL;
+        mutex_unlock(&sock_diag_table_mutex);
+}
+
+static inline struct sock_diag_handler *sock_diag_lock_handler(int family)
+{
+        mutex_lock(&sock_diag_table_mutex);
+        return sock_diag_handlers[family];
+}
+
+static inline void sock_diag_unlock_handler(struct sock_diag_handler *h)
+{
+        mutex_unlock(&sock_diag_table_mutex);
+}
+
 static int __sock_diag_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 {
-        return -EOPNOTSUPP;
+        int err;
+        struct sock_diag_req *req = NLMSG_DATA(nlh);
+        struct sock_diag_handler *hndl;
+
+        if (nlmsg_len(nlh) < sizeof(*req))
+                return -EINVAL;
+
+        hndl = sock_diag_lock_handler(req->sdiag_family);
+        if (hndl == NULL)
+                err = -ENOENT;
+        else
+                err = hndl->dump(skb, nlh);
+        sock_diag_unlock_handler(hndl);
+
+        return err;
 }
 
 static int sock_diag_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
@@ -961,9 +1044,22 @@ static int __init inet_diag_init(void)
                                         sock_diag_rcv, NULL, THIS_MODULE);
         if (sdiagnl == NULL)
                 goto out_free_table;
-        err = 0;
+
+        err = sock_diag_register(&inet_diag_handler);
+        if (err)
+                goto out_free_nl;
+
+        err = sock_diag_register(&inet6_diag_handler);
+        if (err)
+                goto out_free_inet;
+
 out:
         return err;
+
+out_free_inet:
+        sock_diag_unregister(&inet_diag_handler);
+out_free_nl:
+        netlink_kernel_release(sdiagnl);
 out_free_table:
         kfree(inet_diag_table);
         goto out;
@@ -971,6 +1067,8 @@ out_free_table:
 
 static void __exit inet_diag_exit(void)
 {
+        sock_diag_unregister(&inet6_diag_handler);
+        sock_diag_unregister(&inet_diag_handler);
         netlink_kernel_release(sdiagnl);
         kfree(inet_diag_table);
 }