5 files changed, 310 insertions, 17 deletions

diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index 45332acac022..345a15084557 100644
--- a/include/net/xfrm.h
+++ b/include/net/xfrm.h
@@ -1347,6 +1347,18 @@ struct xfrm_algo_desc {
         struct sadb_alg desc;
 };
 
+/* XFRM protocol handlers.  */
+struct xfrm4_protocol {
+        int (*handler)(struct sk_buff *skb);
+        int (*input_handler)(struct sk_buff *skb, int nexthdr, __be32 spi,
+                             int encap_type);
+        int (*cb_handler)(struct sk_buff *skb, int err);
+        int (*err_handler)(struct sk_buff *skb, u32 info);
+
+        struct xfrm4_protocol __rcu *next;
+        int priority;
+};
+
 /* XFRM tunnel handlers.  */
 struct xfrm_tunnel {
         int (*handler)(struct sk_buff *skb);
@@ -1498,13 +1510,18 @@ int xfrm4_rcv(struct sk_buff *skb);
 
 static inline int xfrm4_rcv_spi(struct sk_buff *skb, int nexthdr, __be32 spi)
 {
-        return xfrm4_rcv_encap(skb, nexthdr, spi, 0);
+        XFRM_SPI_SKB_CB(skb)->family = AF_INET;
+        XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
+        return xfrm_input(skb, nexthdr, spi, 0);
 }
 
 int xfrm4_extract_output(struct xfrm_state *x, struct sk_buff *skb);
 int xfrm4_prepare_output(struct xfrm_state *x, struct sk_buff *skb);
 int xfrm4_output(struct sk_buff *skb);
 int xfrm4_output_finish(struct sk_buff *skb);
+int xfrm4_rcv_cb(struct sk_buff *skb, u8 protocol, int err);
+int xfrm4_protocol_register(struct xfrm4_protocol *handler, unsigned char protocol);
+int xfrm4_protocol_deregister(struct xfrm4_protocol *handler, unsigned char protocol);
 int xfrm4_tunnel_register(struct xfrm_tunnel *handler, unsigned short family);
 int xfrm4_tunnel_deregister(struct xfrm_tunnel *handler, unsigned short family);
 void xfrm4_local_error(struct sk_buff *skb, u32 mtu);
@@ -1752,4 +1769,16 @@ static inline int xfrm_mark_put(struct sk_buff *skb, const struct xfrm_mark *m)
         return ret;
 }
 
+static inline int xfrm_rcv_cb(struct sk_buff *skb, unsigned int family,
+                              u8 protocol, int err)
+{
+        switch(family) {
+#ifdef CONFIG_INET
+        case AF_INET:
+                return xfrm4_rcv_cb(skb, protocol, err);
+#endif
+        }
+        return 0;
+}
+
 #endif  /* _NET_XFRM_H */
diff --git a/net/ipv4/Makefile b/net/ipv4/Makefile
index f8c49ce5b283..f032688d20d3 100644
--- a/net/ipv4/Makefile
+++ b/net/ipv4/Makefile
@@ -55,4 +55,4 @@ obj-$(CONFIG_MEMCG_KMEM) += tcp_memcontrol.o
 obj-$(CONFIG_NETLABEL) += cipso_ipv4.o
 
 obj-$(CONFIG_XFRM) += xfrm4_policy.o xfrm4_state.o xfrm4_input.o \
-                      xfrm4_output.o
+                      xfrm4_output.o xfrm4_protocol.o
diff --git a/net/ipv4/xfrm4_input.c b/net/ipv4/xfrm4_input.c
index 1f12c8b45864..aac6197b7a71 100644
--- a/net/ipv4/xfrm4_input.c
+++ b/net/ipv4/xfrm4_input.c
@@ -37,15 +37,6 @@ drop:
         return NET_RX_DROP;
 }
 
-int xfrm4_rcv_encap(struct sk_buff *skb, int nexthdr, __be32 spi,
-                    int encap_type)
-{
-        XFRM_SPI_SKB_CB(skb)->family = AF_INET;
-        XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
-        return xfrm_input(skb, nexthdr, spi, encap_type);
-}
-EXPORT_SYMBOL(xfrm4_rcv_encap);
-
 int xfrm4_transport_finish(struct sk_buff *skb, int async)
 {
         struct iphdr *iph = ip_hdr(skb);
diff --git a/net/ipv4/xfrm4_protocol.c b/net/ipv4/xfrm4_protocol.c
new file mode 100644
index 000000000000..862a26c2014f
--- /dev/null
+++ b/net/ipv4/xfrm4_protocol.c
@@ -0,0 +1,268 @@
+/* xfrm4_protocol.c - Generic xfrm protocol multiplexer.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Author:
+ * Steffen Klassert <steffen.klassert@secunet.com>
+ *
+ * Based on:
+ * net/ipv4/tunnel4.c
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License
+ *      as published by the Free Software Foundation; either version
+ *      2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/init.h>
+#include <linux/mutex.h>
+#include <linux/skbuff.h>
+#include <net/icmp.h>
+#include <net/ip.h>
+#include <net/protocol.h>
+#include <net/xfrm.h>
+
+static struct xfrm4_protocol __rcu *esp4_handlers __read_mostly;
+static struct xfrm4_protocol __rcu *ah4_handlers __read_mostly;
+static struct xfrm4_protocol __rcu *ipcomp4_handlers __read_mostly;
+static DEFINE_MUTEX(xfrm4_protocol_mutex);
+
+static inline struct xfrm4_protocol __rcu **proto_handlers(u8 protocol)
+{
+        switch (protocol) {
+        case IPPROTO_ESP:
+                return &esp4_handlers;
+        case IPPROTO_AH:
+                return &ah4_handlers;
+        case IPPROTO_COMP:
+                return &ipcomp4_handlers;
+        }
+
+        return NULL;
+}
+
+#define for_each_protocol_rcu(head, handler)            \
+        for (handler = rcu_dereference(head);           \
+             handler != NULL;                           \
+             handler = rcu_dereference(handler->next))  \
+
+int xfrm4_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
+{
+        int ret;
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(*proto_handlers(protocol), handler)
+                if ((ret = handler->cb_handler(skb, err)) <= 0)
+                        return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL(xfrm4_rcv_cb);
+
+int xfrm4_rcv_encap(struct sk_buff *skb, int nexthdr, __be32 spi,
+                    int encap_type)
+{
+        int ret;
+        struct xfrm4_protocol *handler;
+
+        XFRM_SPI_SKB_CB(skb)->family = AF_INET;
+        XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
+
+        for_each_protocol_rcu(*proto_handlers(nexthdr), handler)
+                if ((ret = handler->input_handler(skb, nexthdr, spi, encap_type)) != -EINVAL)
+                        return ret;
+
+        icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+EXPORT_SYMBOL(xfrm4_rcv_encap);
+
+static int xfrm4_esp_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(esp4_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;
+
+        icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm4_esp_err(struct sk_buff *skb, u32 info)
+{
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(esp4_handlers, handler)
+                if (!handler->err_handler(skb, info))
+                        break;
+}
+
+static int xfrm4_ah_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(ah4_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;;
+
+        icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm4_ah_err(struct sk_buff *skb, u32 info)
+{
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(ah4_handlers, handler)
+                if (!handler->err_handler(skb, info))
+                        break;
+}
+
+static int xfrm4_ipcomp_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(ipcomp4_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;
+
+        icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm4_ipcomp_err(struct sk_buff *skb, u32 info)
+{
+        struct xfrm4_protocol *handler;
+
+        for_each_protocol_rcu(ipcomp4_handlers, handler)
+                if (!handler->err_handler(skb, info))
+                        break;
+}
+
+static const struct net_protocol esp4_protocol = {
+        .handler        =       xfrm4_esp_rcv,
+        .err_handler    =       xfrm4_esp_err,
+        .no_policy      =       1,
+        .netns_ok       =       1,
+};
+
+static const struct net_protocol ah4_protocol = {
+        .handler        =       xfrm4_ah_rcv,
+        .err_handler    =       xfrm4_ah_err,
+        .no_policy      =       1,
+        .netns_ok       =       1,
+};
+
+static const struct net_protocol ipcomp4_protocol = {
+        .handler        =       xfrm4_ipcomp_rcv,
+        .err_handler    =       xfrm4_ipcomp_err,
+        .no_policy      =       1,
+        .netns_ok       =       1,
+};
+
+static inline const struct net_protocol *netproto(unsigned char protocol)
+{
+        switch (protocol) {
+        case IPPROTO_ESP:
+                return &esp4_protocol;
+        case IPPROTO_AH:
+                return &ah4_protocol;
+        case IPPROTO_COMP:
+                return &ipcomp4_protocol;
+        }
+
+        return NULL;
+}
+
+int xfrm4_protocol_register(struct xfrm4_protocol *handler,
+                            unsigned char protocol)
+{
+        struct xfrm4_protocol __rcu **pprev;
+        struct xfrm4_protocol *t;
+        bool add_netproto = false;
+
+        int ret = -EEXIST;
+        int priority = handler->priority;
+
+        mutex_lock(&xfrm4_protocol_mutex);
+
+        if (!rcu_dereference_protected(*proto_handlers(protocol),
+                                       lockdep_is_held(&xfrm4_protocol_mutex)))
+                add_netproto = true;
+
+        for (pprev = proto_handlers(protocol);
+             (t = rcu_dereference_protected(*pprev,
+                        lockdep_is_held(&xfrm4_protocol_mutex))) != NULL;
+             pprev = &t->next) {
+                if (t->priority < priority)
+                        break;
+                if (t->priority == priority)
+                        goto err;
+        }
+
+        handler->next = *pprev;
+        rcu_assign_pointer(*pprev, handler);
+
+        ret = 0;
+
+err:
+        mutex_unlock(&xfrm4_protocol_mutex);
+
+        if (add_netproto) {
+                if (inet_add_protocol(netproto(protocol), protocol)) {
+                        pr_err("%s: can't add protocol\n", __func__);
+                        ret = -EAGAIN;
+                }
+        }
+
+        return ret;
+}
+EXPORT_SYMBOL(xfrm4_protocol_register);
+
+int xfrm4_protocol_deregister(struct xfrm4_protocol *handler,
+                              unsigned char protocol)
+{
+        struct xfrm4_protocol __rcu **pprev;
+        struct xfrm4_protocol *t;
+        int ret = -ENOENT;
+
+        mutex_lock(&xfrm4_protocol_mutex);
+
+        for (pprev = proto_handlers(protocol);
+             (t = rcu_dereference_protected(*pprev,
+                        lockdep_is_held(&xfrm4_protocol_mutex))) != NULL;
+             pprev = &t->next) {
+                if (t == handler) {
+                        *pprev = handler->next;
+                        ret = 0;
+                        break;
+                }
+        }
+
+        if (!rcu_dereference_protected(*proto_handlers(protocol),
+                                       lockdep_is_held(&xfrm4_protocol_mutex))) {
+                if (inet_del_protocol(netproto(protocol), protocol) < 0) {
+                        pr_err("%s: can't remove protocol\n", __func__);
+                        ret = -EAGAIN;
+                }
+        }
+
+        mutex_unlock(&xfrm4_protocol_mutex);
+
+        synchronize_net();
+
+        return ret;
+}
+EXPORT_SYMBOL(xfrm4_protocol_deregister);
diff --git a/net/xfrm/xfrm_input.c b/net/xfrm/xfrm_input.c
index 6c7ac016ce3a..99e3a9e5285e 100644
--- a/net/xfrm/xfrm_input.c
+++ b/net/xfrm/xfrm_input.c
@@ -108,7 +108,7 @@ int xfrm_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
         int err;
         __be32 seq;
         __be32 seq_hi;
-        struct xfrm_state *x;
+        struct xfrm_state *x = NULL;
         xfrm_address_t *daddr;
         struct xfrm_mode *inner_mode;
         unsigned int family;
@@ -120,9 +120,14 @@ int xfrm_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
                 async = 1;
                 x = xfrm_input_state(skb);
                 seq = XFRM_SKB_CB(skb)->seq.input.low;
+                family = x->outer_mode->afinfo->family;
                 goto resume;
         }
 
+        daddr = (xfrm_address_t *)(skb_network_header(skb) +
+                                   XFRM_SPI_SKB_CB(skb)->daddroff);
+        family = XFRM_SPI_SKB_CB(skb)->family;
+
         /* Allocate new secpath or COW existing one. */
         if (!skb->sp || atomic_read(&skb->sp->refcnt) != 1) {
                 struct sec_path *sp;
@@ -137,10 +142,6 @@ int xfrm_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
                 skb->sp = sp;
         }
 
-        daddr = (xfrm_address_t *)(skb_network_header(skb) +
-                                   XFRM_SPI_SKB_CB(skb)->daddroff);
-        family = XFRM_SPI_SKB_CB(skb)->family;
-
         seq = 0;
         if (!spi && (err = xfrm_parse_spi(skb, nexthdr, &spi, &seq)) != 0) {
                 XFRM_INC_STATS(net, LINUX_MIB_XFRMINHDRERROR);
@@ -201,7 +202,6 @@ int xfrm_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
 
                 if (nexthdr == -EINPROGRESS)
                         return 0;
-
 resume:
                 spin_lock(&x->lock);
                 if (nexthdr <= 0) {
@@ -263,6 +263,10 @@ resume:
                 }
         } while (!err);
 
+        err = xfrm_rcv_cb(skb, family, x->type->proto, 0);
+        if (err)
+                goto drop;
+
         nf_reset(skb);
 
         if (decaps) {
@@ -276,6 +280,7 @@ resume:
 drop_unlock:
         spin_unlock(&x->lock);
 drop:
+        xfrm_rcv_cb(skb, family, x && x->type ? x->type->proto : nexthdr, -1);
         kfree_skb(skb);
         return 0;
 }