16 files changed, 668 insertions, 211 deletions

diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index 23bfd4591e8b..32682ae47b3f 100644
--- a/include/net/xfrm.h
+++ b/include/net/xfrm.h
@@ -121,7 +121,7 @@ struct xfrm_state_walk {
         u8                      dying;
         u8                      proto;
         u32                     seq;
-        struct xfrm_filter      *filter;
+        struct xfrm_address_filter *filter;
 };
 
 /* Full description of state of transformer. */
@@ -349,6 +349,16 @@ int xfrm_state_unregister_afinfo(struct xfrm_state_afinfo *afinfo);
 struct xfrm_state_afinfo *xfrm_state_get_afinfo(unsigned int family);
 void xfrm_state_put_afinfo(struct xfrm_state_afinfo *afinfo);
 
+struct xfrm_input_afinfo {
+        unsigned int            family;
+        struct module           *owner;
+        int                     (*callback)(struct sk_buff *skb, u8 protocol,
+                                            int err);
+};
+
+int xfrm_input_register_afinfo(struct xfrm_input_afinfo *afinfo);
+int xfrm_input_unregister_afinfo(struct xfrm_input_afinfo *afinfo);
+
 void xfrm_state_delete_tunnel(struct xfrm_state *x);
 
 struct xfrm_type {
@@ -1364,18 +1374,22 @@ struct xfrm4_protocol {
         int priority;
 };
 
-/* XFRM tunnel handlers.  */
-struct xfrm_tunnel {
+struct xfrm6_protocol {
         int (*handler)(struct sk_buff *skb);
-        int (*err_handler)(struct sk_buff *skb, u32 info);
+        int (*cb_handler)(struct sk_buff *skb, int err);
+        int (*err_handler)(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                           u8 type, u8 code, int offset, __be32 info);
 
-        struct xfrm_tunnel __rcu *next;
+        struct xfrm6_protocol __rcu *next;
         int priority;
 };
 
-struct xfrm_tunnel_notifier {
+/* XFRM tunnel handlers.  */
+struct xfrm_tunnel {
         int (*handler)(struct sk_buff *skb);
-        struct xfrm_tunnel_notifier __rcu *next;
+        int (*err_handler)(struct sk_buff *skb, u32 info);
+
+        struct xfrm_tunnel __rcu *next;
         int priority;
 };
 
@@ -1392,11 +1406,14 @@ void xfrm4_init(void);
 int xfrm_state_init(struct net *net);
 void xfrm_state_fini(struct net *net);
 void xfrm4_state_init(void);
+void xfrm4_protocol_init(void);
 #ifdef CONFIG_XFRM
 int xfrm6_init(void);
 void xfrm6_fini(void);
 int xfrm6_state_init(void);
 void xfrm6_state_fini(void);
+int xfrm6_protocol_init(void);
+void xfrm6_protocol_fini(void);
 #else
 static inline int xfrm6_init(void)
 {
@@ -1423,7 +1440,7 @@ static inline void xfrm_sysctl_fini(struct net *net)
 #endif
 
 void xfrm_state_walk_init(struct xfrm_state_walk *walk, u8 proto,
-                          struct xfrm_filter *filter);
+                          struct xfrm_address_filter *filter);
 int xfrm_state_walk(struct net *net, struct xfrm_state_walk *walk,
                     int (*func)(struct xfrm_state *, int, void*), void *);
 void xfrm_state_walk_done(struct xfrm_state_walk *walk, struct net *net);
@@ -1531,8 +1548,6 @@ int xfrm4_protocol_deregister(struct xfrm4_protocol *handler, unsigned char prot
 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);
-int xfrm6_mode_tunnel_input_register(struct xfrm_tunnel_notifier *handler);
-int xfrm6_mode_tunnel_input_deregister(struct xfrm_tunnel_notifier *handler);
 int xfrm6_extract_header(struct sk_buff *skb);
 int xfrm6_extract_input(struct xfrm_state *x, struct sk_buff *skb);
 int xfrm6_rcv_spi(struct sk_buff *skb, int nexthdr, __be32 spi);
@@ -1541,6 +1556,9 @@ int xfrm6_rcv(struct sk_buff *skb);
 int xfrm6_input_addr(struct sk_buff *skb, xfrm_address_t *daddr,
                      xfrm_address_t *saddr, u8 proto);
 void xfrm6_local_error(struct sk_buff *skb, u32 mtu);
+int xfrm6_rcv_cb(struct sk_buff *skb, u8 protocol, int err);
+int xfrm6_protocol_register(struct xfrm6_protocol *handler, unsigned char protocol);
+int xfrm6_protocol_deregister(struct xfrm6_protocol *handler, unsigned char protocol);
 int xfrm6_tunnel_register(struct xfrm6_tunnel *handler, unsigned short family);
 int xfrm6_tunnel_deregister(struct xfrm6_tunnel *handler, unsigned short family);
 __be32 xfrm6_tunnel_alloc_spi(struct net *net, xfrm_address_t *saddr);
@@ -1784,18 +1802,6 @@ 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;
-}
-
 static inline int xfrm_tunnel_check(struct sk_buff *skb, struct xfrm_state *x,
                                     unsigned int family)
 {
diff --git a/include/uapi/linux/xfrm.h b/include/uapi/linux/xfrm.h
index 6550c679584f..25e5dd916ba4 100644
--- a/include/uapi/linux/xfrm.h
+++ b/include/uapi/linux/xfrm.h
@@ -299,7 +299,7 @@ enum xfrm_attr_type_t {
         XFRMA_REPLAY_ESN_VAL,   /* struct xfrm_replay_esn */
         XFRMA_SA_EXTRA_FLAGS,   /* __u32 */
         XFRMA_PROTO,            /* __u8 */
-        XFRMA_FILTER,           /* struct xfrm_filter */
+        XFRMA_ADDRESS_FILTER,   /* struct xfrm_address_filter */
         __XFRMA_MAX
 
 #define XFRMA_MAX (__XFRMA_MAX - 1)
@@ -476,7 +476,7 @@ struct xfrm_user_mapping {
         __be16                          new_sport;
 };
 
-struct xfrm_filter {
+struct xfrm_address_filter {
         xfrm_address_t                  saddr;
         xfrm_address_t                  daddr;
         __u16                           family;
diff --git a/net/ipv4/xfrm4_policy.c b/net/ipv4/xfrm4_policy.c
index e1a63930a967..6156f68a1e90 100644
--- a/net/ipv4/xfrm4_policy.c
+++ b/net/ipv4/xfrm4_policy.c
@@ -325,6 +325,7 @@ void __init xfrm4_init(void)
 
         xfrm4_state_init();
         xfrm4_policy_init();
+        xfrm4_protocol_init();
 #ifdef CONFIG_SYSCTL
         register_pernet_subsys(&xfrm4_net_ops);
 #endif
diff --git a/net/ipv4/xfrm4_protocol.c b/net/ipv4/xfrm4_protocol.c
index cdc09efca442..7f7b243e8139 100644
--- a/net/ipv4/xfrm4_protocol.c
+++ b/net/ipv4/xfrm4_protocol.c
@@ -179,6 +179,12 @@ static const struct net_protocol ipcomp4_protocol = {
         .netns_ok       =       1,
 };
 
+static struct xfrm_input_afinfo xfrm4_input_afinfo = {
+        .family         =       AF_INET,
+        .owner          =       THIS_MODULE,
+        .callback       =       xfrm4_rcv_cb,
+};
+
 static inline const struct net_protocol *netproto(unsigned char protocol)
 {
         switch (protocol) {
@@ -199,7 +205,6 @@ int xfrm4_protocol_register(struct xfrm4_protocol *handler,
         struct xfrm4_protocol __rcu **pprev;
         struct xfrm4_protocol *t;
         bool add_netproto = false;
-
         int ret = -EEXIST;
         int priority = handler->priority;
 
@@ -273,3 +278,9 @@ int xfrm4_protocol_deregister(struct xfrm4_protocol *handler,
         return ret;
 }
 EXPORT_SYMBOL(xfrm4_protocol_deregister);
+
+void __init xfrm4_protocol_init(void)
+{
+        xfrm_input_register_afinfo(&xfrm4_input_afinfo);
+}
+EXPORT_SYMBOL(xfrm4_protocol_init);
diff --git a/net/ipv6/Makefile b/net/ipv6/Makefile
index 17bb830872db..2fe68364bb20 100644
--- a/net/ipv6/Makefile
+++ b/net/ipv6/Makefile
@@ -16,7 +16,7 @@ ipv6-$(CONFIG_SYSCTL) = sysctl_net_ipv6.o
 ipv6-$(CONFIG_IPV6_MROUTE) += ip6mr.o
 
 ipv6-$(CONFIG_XFRM) += xfrm6_policy.o xfrm6_state.o xfrm6_input.o \
-        xfrm6_output.o
+        xfrm6_output.o xfrm6_protocol.o
 ipv6-$(CONFIG_NETFILTER) += netfilter.o
 ipv6-$(CONFIG_IPV6_MULTIPLE_TABLES) += fib6_rules.o
 ipv6-$(CONFIG_PROC_FS) += proc.o
diff --git a/net/ipv6/ah6.c b/net/ipv6/ah6.c
index 6c5f0949e0ab..72a4930bdc0a 100644
--- a/net/ipv6/ah6.c
+++ b/net/ipv6/ah6.c
@@ -643,8 +643,8 @@ out:
         return err;
 }
 
-static void ah6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
-                    u8 type, u8 code, int offset, __be32 info)
+static int ah6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                   u8 type, u8 code, int offset, __be32 info)
 {
         struct net *net = dev_net(skb->dev);
         struct ipv6hdr *iph = (struct ipv6hdr*)skb->data;
@@ -653,17 +653,19 @@ static void ah6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 
         if (type != ICMPV6_PKT_TOOBIG &&
             type != NDISC_REDIRECT)
-                return;
+                return 0;
 
         x = xfrm_state_lookup(net, skb->mark, (xfrm_address_t *)&iph->daddr, ah->spi, IPPROTO_AH, AF_INET6);
         if (!x)
-                return;
+                return 0;
 
         if (type == NDISC_REDIRECT)
                 ip6_redirect(skb, net, skb->dev->ifindex, 0);
         else
                 ip6_update_pmtu(skb, net, info, 0, 0);
         xfrm_state_put(x);
+
+        return 0;
 }
 
 static int ah6_init_state(struct xfrm_state *x)
@@ -748,6 +750,11 @@ static void ah6_destroy(struct xfrm_state *x)
         kfree(ahp);
 }
 
+static int ah6_rcv_cb(struct sk_buff *skb, int err)
+{
+        return 0;
+}
+
 static const struct xfrm_type ah6_type =
 {
         .description    = "AH6",
@@ -761,10 +768,11 @@ static const struct xfrm_type ah6_type =
         .hdr_offset     = xfrm6_find_1stfragopt,
 };
 
-static const struct inet6_protocol ah6_protocol = {
+static struct xfrm6_protocol ah6_protocol = {
         .handler        =       xfrm6_rcv,
+        .cb_handler     =       ah6_rcv_cb,
         .err_handler    =       ah6_err,
-        .flags          =       INET6_PROTO_NOPOLICY,
+        .priority       =       0,
 };
 
 static int __init ah6_init(void)
@@ -774,7 +782,7 @@ static int __init ah6_init(void)
                 return -EAGAIN;
         }
 
-        if (inet6_add_protocol(&ah6_protocol, IPPROTO_AH) < 0) {
+        if (xfrm6_protocol_register(&ah6_protocol, IPPROTO_AH) < 0) {
                 pr_info("%s: can't add protocol\n", __func__);
                 xfrm_unregister_type(&ah6_type, AF_INET6);
                 return -EAGAIN;
@@ -785,7 +793,7 @@ static int __init ah6_init(void)
 
 static void __exit ah6_fini(void)
 {
-        if (inet6_del_protocol(&ah6_protocol, IPPROTO_AH) < 0)
+        if (xfrm6_protocol_deregister(&ah6_protocol, IPPROTO_AH) < 0)
                 pr_info("%s: can't remove protocol\n", __func__);
 
         if (xfrm_unregister_type(&ah6_type, AF_INET6) < 0)
diff --git a/net/ipv6/esp6.c b/net/ipv6/esp6.c
index 6eef8a7e35f2..d15da1377149 100644
--- a/net/ipv6/esp6.c
+++ b/net/ipv6/esp6.c
@@ -421,8 +421,8 @@ static u32 esp6_get_mtu(struct xfrm_state *x, int mtu)
                  net_adj) & ~(blksize - 1)) + net_adj - 2;
 }
 
-static void esp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
-                     u8 type, u8 code, int offset, __be32 info)
+static int esp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                    u8 type, u8 code, int offset, __be32 info)
 {
         struct net *net = dev_net(skb->dev);
         const struct ipv6hdr *iph = (const struct ipv6hdr *)skb->data;
@@ -431,18 +431,20 @@ static void esp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 
         if (type != ICMPV6_PKT_TOOBIG &&
             type != NDISC_REDIRECT)
-                return;
+                return 0;
 
         x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
                               esph->spi, IPPROTO_ESP, AF_INET6);
         if (!x)
-                return;
+                return 0;
 
         if (type == NDISC_REDIRECT)
                 ip6_redirect(skb, net, skb->dev->ifindex, 0);
         else
                 ip6_update_pmtu(skb, net, info, 0, 0);
         xfrm_state_put(x);
+
+        return 0;
 }
 
 static void esp6_destroy(struct xfrm_state *x)
@@ -614,6 +616,11 @@ error:
         return err;
 }
 
+static int esp6_rcv_cb(struct sk_buff *skb, int err)
+{
+        return 0;
+}
+
 static const struct xfrm_type esp6_type =
 {
         .description    = "ESP6",
@@ -628,10 +635,11 @@ static const struct xfrm_type esp6_type =
         .hdr_offset     = xfrm6_find_1stfragopt,
 };
 
-static const struct inet6_protocol esp6_protocol = {
-        .handler        =       xfrm6_rcv,
+static struct xfrm6_protocol esp6_protocol = {
+        .handler        =       xfrm6_rcv,
+        .cb_handler     =       esp6_rcv_cb,
         .err_handler    =       esp6_err,
-        .flags          =       INET6_PROTO_NOPOLICY,
+        .priority       =       0,
 };
 
 static int __init esp6_init(void)
@@ -640,7 +648,7 @@ static int __init esp6_init(void)
                 pr_info("%s: can't add xfrm type\n", __func__);
                 return -EAGAIN;
         }
-        if (inet6_add_protocol(&esp6_protocol, IPPROTO_ESP) < 0) {
+        if (xfrm6_protocol_register(&esp6_protocol, IPPROTO_ESP) < 0) {
                 pr_info("%s: can't add protocol\n", __func__);
                 xfrm_unregister_type(&esp6_type, AF_INET6);
                 return -EAGAIN;
@@ -651,7 +659,7 @@ static int __init esp6_init(void)
 
 static void __exit esp6_fini(void)
 {
-        if (inet6_del_protocol(&esp6_protocol, IPPROTO_ESP) < 0)
+        if (xfrm6_protocol_deregister(&esp6_protocol, IPPROTO_ESP) < 0)
                 pr_info("%s: can't remove protocol\n", __func__);
         if (xfrm_unregister_type(&esp6_type, AF_INET6) < 0)
                 pr_info("%s: can't remove xfrm type\n", __func__);
diff --git a/net/ipv6/ip6_vti.c b/net/ipv6/ip6_vti.c
index 864914399391..b7c0f827140b 100644
--- a/net/ipv6/ip6_vti.c
+++ b/net/ipv6/ip6_vti.c
@@ -278,7 +278,6 @@ static void vti6_dev_uninit(struct net_device *dev)
                 RCU_INIT_POINTER(ip6n->tnls_wc[0], NULL);
         else
                 vti6_tnl_unlink(ip6n, t);
-        ip6_tnl_dst_reset(t);
         dev_put(dev);
 }
 
@@ -288,11 +287,8 @@ static int vti6_rcv(struct sk_buff *skb)
         const struct ipv6hdr *ipv6h = ipv6_hdr(skb);
 
         rcu_read_lock();
-
         if ((t = vti6_tnl_lookup(dev_net(skb->dev), &ipv6h->saddr,
                                  &ipv6h->daddr)) != NULL) {
-                struct pcpu_sw_netstats *tstats;
-
                 if (t->parms.proto != IPPROTO_IPV6 && t->parms.proto != 0) {
                         rcu_read_unlock();
                         goto discard;
@@ -309,27 +305,58 @@ static int vti6_rcv(struct sk_buff *skb)
                         goto discard;
                 }
 
-                tstats = this_cpu_ptr(t->dev->tstats);
-                u64_stats_update_begin(&tstats->syncp);
-                tstats->rx_packets++;
-                tstats->rx_bytes += skb->len;
-                u64_stats_update_end(&tstats->syncp);
-
-                skb->mark = 0;
-                secpath_reset(skb);
-                skb->dev = t->dev;
+                XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = t;
+                skb->mark = be32_to_cpu(t->parms.i_key);
 
                 rcu_read_unlock();
-                return 0;
+
+                return xfrm6_rcv(skb);
         }
         rcu_read_unlock();
-        return 1;
-
+        return -EINVAL;
 discard:
         kfree_skb(skb);
         return 0;
 }
 
+static int vti6_rcv_cb(struct sk_buff *skb, int err)
+{
+        unsigned short family;
+        struct net_device *dev;
+        struct pcpu_sw_netstats *tstats;
+        struct xfrm_state *x;
+        struct ip6_tnl *t = XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6;
+
+        if (!t)
+                return 1;
+
+        dev = t->dev;
+
+        if (err) {
+                dev->stats.rx_errors++;
+                dev->stats.rx_dropped++;
+
+                return 0;
+        }
+
+        x = xfrm_input_state(skb);
+        family = x->inner_mode->afinfo->family;
+
+        if (!xfrm_policy_check(NULL, XFRM_POLICY_IN, skb, family))
+                return -EPERM;
+
+        skb_scrub_packet(skb, !net_eq(t->net, dev_net(skb->dev)));
+        skb->dev = dev;
+
+        tstats = this_cpu_ptr(dev->tstats);
+        u64_stats_update_begin(&tstats->syncp);
+        tstats->rx_packets++;
+        tstats->rx_bytes += skb->len;
+        u64_stats_update_end(&tstats->syncp);
+
+        return 0;
+}
+
 /**
  * vti6_addr_conflict - compare packet addresses to tunnel's own
  *   @t: the outgoing tunnel device
@@ -349,44 +376,56 @@ vti6_addr_conflict(const struct ip6_tnl *t, const struct ipv6hdr *hdr)
         return ipv6_addr_equal(&t->parms.raddr, &hdr->saddr);
 }
 
+static bool vti6_state_check(const struct xfrm_state *x,
+                             const struct in6_addr *dst,
+                             const struct in6_addr *src)
+{
+        xfrm_address_t *daddr = (xfrm_address_t *)dst;
+        xfrm_address_t *saddr = (xfrm_address_t *)src;
+
+        /* if there is no transform then this tunnel is not functional.
+         * Or if the xfrm is not mode tunnel.
+         */
+        if (!x || x->props.mode != XFRM_MODE_TUNNEL ||
+            x->props.family != AF_INET6)
+                return false;
+
+        if (ipv6_addr_any(dst))
+                return xfrm_addr_equal(saddr, &x->props.saddr, AF_INET6);
+
+        if (!xfrm_state_addr_check(x, daddr, saddr, AF_INET6))
+                return false;
+
+        return true;
+}
+
 /**
  * vti6_xmit - send a packet
  *   @skb: the outgoing socket buffer
  *   @dev: the outgoing tunnel device
+ *   @fl: the flow informations for the xfrm_lookup
  **/
-static int vti6_xmit(struct sk_buff *skb, struct net_device *dev)
+static int
+vti6_xmit(struct sk_buff *skb, struct net_device *dev, struct flowi *fl)
 {
-        struct net *net = dev_net(dev);
         struct ip6_tnl *t = netdev_priv(dev);
         struct net_device_stats *stats = &t->dev->stats;
-        struct dst_entry *dst = NULL, *ndst = NULL;
-        struct flowi6 fl6;
-        struct ipv6hdr *ipv6h = ipv6_hdr(skb);
+        struct dst_entry *dst = skb_dst(skb);
         struct net_device *tdev;
         int err = -1;
 
-        if ((t->parms.proto != IPPROTO_IPV6 && t->parms.proto != 0) ||
-            !ip6_tnl_xmit_ctl(t) || vti6_addr_conflict(t, ipv6h))
-                return err;
-
-        dst = ip6_tnl_dst_check(t);
-        if (!dst) {
-                memcpy(&fl6, &t->fl.u.ip6, sizeof(fl6));
-
-                ndst = ip6_route_output(net, NULL, &fl6);
+        if (!dst)
+                goto tx_err_link_failure;
 
-                if (ndst->error)
-                        goto tx_err_link_failure;
-                ndst = xfrm_lookup(net, ndst, flowi6_to_flowi(&fl6), NULL, 0);
-                if (IS_ERR(ndst)) {
-                        err = PTR_ERR(ndst);
-                        ndst = NULL;
-                        goto tx_err_link_failure;
-                }
-                dst = ndst;
+        dst_hold(dst);
+        dst = xfrm_lookup(t->net, dst, fl, NULL, 0);
+        if (IS_ERR(dst)) {
+                err = PTR_ERR(dst);
+                dst = NULL;
+                goto tx_err_link_failure;
         }
 
-        if (!dst->xfrm || dst->xfrm->props.mode != XFRM_MODE_TUNNEL)
+        if (!vti6_state_check(dst->xfrm, &t->parms.raddr, &t->parms.laddr))
                 goto tx_err_link_failure;
 
         tdev = dst->dev;
@@ -398,14 +437,21 @@ static int vti6_xmit(struct sk_buff *skb, struct net_device *dev)
                 goto tx_err_dst_release;
         }
 
+        skb_scrub_packet(skb, !net_eq(t->net, dev_net(dev)));
+        skb_dst_set(skb, dst);
+        skb->dev = skb_dst(skb)->dev;
 
-        skb_dst_drop(skb);
-        skb_dst_set_noref(skb, dst);
+        err = dst_output(skb);
+        if (net_xmit_eval(err) == 0) {
+                struct pcpu_sw_netstats *tstats = this_cpu_ptr(dev->tstats);
 
-        ip6tunnel_xmit(skb, dev);
-        if (ndst) {
-                dev->mtu = dst_mtu(ndst);
-                ip6_tnl_dst_store(t, ndst);
+                u64_stats_update_begin(&tstats->syncp);
+                tstats->tx_bytes += skb->len;
+                tstats->tx_packets++;
+                u64_stats_update_end(&tstats->syncp);
+        } else {
+                stats->tx_errors++;
+                stats->tx_aborted_errors++;
         }
 
         return 0;
@@ -413,7 +459,7 @@ tx_err_link_failure:
         stats->tx_carrier_errors++;
         dst_link_failure(skb);
 tx_err_dst_release:
-        dst_release(ndst);
+        dst_release(dst);
         return err;
 }
 
@@ -422,16 +468,33 @@ vti6_tnl_xmit(struct sk_buff *skb, struct net_device *dev)
 {
         struct ip6_tnl *t = netdev_priv(dev);
         struct net_device_stats *stats = &t->dev->stats;
+        struct ipv6hdr *ipv6h;
+        struct flowi fl;
         int ret;
 
+        memset(&fl, 0, sizeof(fl));
+        skb->mark = be32_to_cpu(t->parms.o_key);
+
         switch (skb->protocol) {
         case htons(ETH_P_IPV6):
-                ret = vti6_xmit(skb, dev);
+                ipv6h = ipv6_hdr(skb);
+
+                if ((t->parms.proto != IPPROTO_IPV6 && t->parms.proto != 0) ||
+                    !ip6_tnl_xmit_ctl(t) || vti6_addr_conflict(t, ipv6h))
+                        goto tx_err;
+
+                xfrm_decode_session(skb, &fl, AF_INET6);
+                memset(IP6CB(skb), 0, sizeof(*IP6CB(skb)));
+                break;
+        case htons(ETH_P_IP):
+                xfrm_decode_session(skb, &fl, AF_INET);
+                memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
                 break;
         default:
                 goto tx_err;
         }
 
+        ret = vti6_xmit(skb, dev, &fl);
         if (ret < 0)
                 goto tx_err;
 
@@ -444,24 +507,66 @@ tx_err:
         return NETDEV_TX_OK;
 }
 
+static int vti6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                    u8 type, u8 code, int offset, __be32 info)
+{
+        __be32 spi;
+        struct xfrm_state *x;
+        struct ip6_tnl *t;
+        struct ip_esp_hdr *esph;
+        struct ip_auth_hdr *ah;
+        struct ip_comp_hdr *ipch;
+        struct net *net = dev_net(skb->dev);
+        const struct ipv6hdr *iph = (const struct ipv6hdr *)skb->data;
+        int protocol = iph->nexthdr;
+
+        t = vti6_tnl_lookup(dev_net(skb->dev), &iph->daddr, &iph->saddr);
+        if (!t)
+                return -1;
+
+        switch (protocol) {
+        case IPPROTO_ESP:
+                esph = (struct ip_esp_hdr *)(skb->data + offset);
+                spi = esph->spi;
+                break;
+        case IPPROTO_AH:
+                ah = (struct ip_auth_hdr *)(skb->data + offset);
+                spi = ah->spi;
+                break;
+        case IPPROTO_COMP:
+                ipch = (struct ip_comp_hdr *)(skb->data + offset);
+                spi = htonl(ntohs(ipch->cpi));
+                break;
+        default:
+                return 0;
+        }
+
+        if (type != ICMPV6_PKT_TOOBIG &&
+            type != NDISC_REDIRECT)
+                return 0;
+
+        x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
+                              spi, protocol, AF_INET6);
+        if (!x)
+                return 0;
+
+        if (type == NDISC_REDIRECT)
+                ip6_redirect(skb, net, skb->dev->ifindex, 0);
+        else
+                ip6_update_pmtu(skb, net, info, 0, 0);
+        xfrm_state_put(x);
+
+        return 0;
+}
+
 static void vti6_link_config(struct ip6_tnl *t)
 {
-        struct dst_entry *dst;
         struct net_device *dev = t->dev;
         struct __ip6_tnl_parm *p = &t->parms;
-        struct flowi6 *fl6 = &t->fl.u.ip6;
 
         memcpy(dev->dev_addr, &p->laddr, sizeof(struct in6_addr));
         memcpy(dev->broadcast, &p->raddr, sizeof(struct in6_addr));
 
-        /* Set up flowi template */
-        fl6->saddr = p->laddr;
-        fl6->daddr = p->raddr;
-        fl6->flowi6_oif = p->link;
-        fl6->flowi6_mark = be32_to_cpu(p->i_key);
-        fl6->flowi6_proto = p->proto;
-        fl6->flowlabel = 0;
-
         p->flags &= ~(IP6_TNL_F_CAP_XMIT | IP6_TNL_F_CAP_RCV |
                       IP6_TNL_F_CAP_PER_PACKET);
         p->flags |= ip6_tnl_get_cap(t, &p->laddr, &p->raddr);
@@ -472,28 +577,6 @@ static void vti6_link_config(struct ip6_tnl *t)
                 dev->flags &= ~IFF_POINTOPOINT;
 
         dev->iflink = p->link;
-
-        if (p->flags & IP6_TNL_F_CAP_XMIT) {
-
-                dst = ip6_route_output(dev_net(dev), NULL, fl6);
-                if (dst->error)
-                        return;
-
-                dst = xfrm_lookup(dev_net(dev), dst, flowi6_to_flowi(fl6),
-                                  NULL, 0);
-                if (IS_ERR(dst))
-                        return;
-
-                if (dst->dev) {
-                        dev->hard_header_len = dst->dev->hard_header_len;
-
-                        dev->mtu = dst_mtu(dst);
-
-                        if (dev->mtu < IPV6_MIN_MTU)
-                                dev->mtu = IPV6_MIN_MTU;
-                }
-                dst_release(dst);
-        }
 }
 
 /**
@@ -720,7 +803,6 @@ static void vti6_dev_setup(struct net_device *dev)
         t = netdev_priv(dev);
         dev->flags |= IFF_NOARP;
         dev->addr_len = sizeof(struct in6_addr);
-        dev->features |= NETIF_F_NETNS_LOCAL;
         dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
 }
 
@@ -908,11 +990,6 @@ static struct rtnl_link_ops vti6_link_ops __read_mostly = {
         .fill_info      = vti6_fill_info,
 };
 
-static struct xfrm_tunnel_notifier vti6_handler __read_mostly = {
-        .handler        = vti6_rcv,
-        .priority       =       1,
-};
-
 static void __net_exit vti6_destroy_tunnels(struct vti6_net *ip6n)
 {
         int h;
@@ -984,6 +1061,27 @@ static struct pernet_operations vti6_net_ops = {
         .size = sizeof(struct vti6_net),
 };
 
+static struct xfrm6_protocol vti_esp6_protocol __read_mostly = {
+        .handler        =       vti6_rcv,
+        .cb_handler     =       vti6_rcv_cb,
+        .err_handler    =       vti6_err,
+        .priority       =       100,
+};
+
+static struct xfrm6_protocol vti_ah6_protocol __read_mostly = {
+        .handler        =       vti6_rcv,
+        .cb_handler     =       vti6_rcv_cb,
+        .err_handler    =       vti6_err,
+        .priority       =       100,
+};
+
+static struct xfrm6_protocol vti_ipcomp6_protocol __read_mostly = {
+        .handler        =       vti6_rcv,
+        .cb_handler     =       vti6_rcv_cb,
+        .err_handler    =       vti6_err,
+        .priority       =       100,
+};
+
 /**
  * vti6_tunnel_init - register protocol and reserve needed resources
  *
@@ -997,11 +1095,33 @@ static int __init vti6_tunnel_init(void)
         if (err < 0)
                 goto out_pernet;
 
-        err = xfrm6_mode_tunnel_input_register(&vti6_handler);
+        err = xfrm6_protocol_register(&vti_esp6_protocol, IPPROTO_ESP);
         if (err < 0) {
-                pr_err("%s: can't register vti6\n", __func__);
+                unregister_pernet_device(&vti6_net_ops);
+                pr_err("%s: can't register vti6 protocol\n", __func__);
+
                 goto out;
         }
+
+        err = xfrm6_protocol_register(&vti_ah6_protocol, IPPROTO_AH);
+        if (err < 0) {
+                xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP);
+                unregister_pernet_device(&vti6_net_ops);
+                pr_err("%s: can't register vti6 protocol\n", __func__);
+
+                goto out;
+        }
+
+        err = xfrm6_protocol_register(&vti_ipcomp6_protocol, IPPROTO_COMP);
+        if (err < 0) {
+                xfrm6_protocol_deregister(&vti_ah6_protocol, IPPROTO_AH);
+                xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP);
+                unregister_pernet_device(&vti6_net_ops);
+                pr_err("%s: can't register vti6 protocol\n", __func__);
+
+                goto out;
+        }
+
         err = rtnl_link_register(&vti6_link_ops);
         if (err < 0)
                 goto rtnl_link_failed;
@@ -1009,7 +1129,9 @@ static int __init vti6_tunnel_init(void)
         return 0;
 
 rtnl_link_failed:
-        xfrm6_mode_tunnel_input_deregister(&vti6_handler);
+        xfrm6_protocol_deregister(&vti_ipcomp6_protocol, IPPROTO_COMP);
+        xfrm6_protocol_deregister(&vti_ah6_protocol, IPPROTO_AH);
+        xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP);
 out:
         unregister_pernet_device(&vti6_net_ops);
 out_pernet:
@@ -1022,8 +1144,12 @@ out_pernet:
 static void __exit vti6_tunnel_cleanup(void)
 {
         rtnl_link_unregister(&vti6_link_ops);
-        if (xfrm6_mode_tunnel_input_deregister(&vti6_handler))
-                pr_info("%s: can't deregister vti6\n", __func__);
+        if (xfrm6_protocol_deregister(&vti_ipcomp6_protocol, IPPROTO_COMP))
+                pr_info("%s: can't deregister protocol\n", __func__);
+        if (xfrm6_protocol_deregister(&vti_ah6_protocol, IPPROTO_AH))
+                pr_info("%s: can't deregister protocol\n", __func__);
+        if (xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP))
+                pr_info("%s: can't deregister protocol\n", __func__);
 
         unregister_pernet_device(&vti6_net_ops);
 }
diff --git a/net/ipv6/ipcomp6.c b/net/ipv6/ipcomp6.c
index da9becb42e81..d1c793cffcb5 100644
--- a/net/ipv6/ipcomp6.c
+++ b/net/ipv6/ipcomp6.c
@@ -53,7 +53,7 @@
 #include <linux/icmpv6.h>
 #include <linux/mutex.h>
 
-static void ipcomp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+static int ipcomp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
                                 u8 type, u8 code, int offset, __be32 info)
 {
         struct net *net = dev_net(skb->dev);
@@ -65,19 +65,21 @@ static void ipcomp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 
         if (type != ICMPV6_PKT_TOOBIG &&
             type != NDISC_REDIRECT)
-                return;
+                return 0;
 
         spi = htonl(ntohs(ipcomph->cpi));
         x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
                               spi, IPPROTO_COMP, AF_INET6);
         if (!x)
-                return;
+                return 0;
 
         if (type == NDISC_REDIRECT)
                 ip6_redirect(skb, net, skb->dev->ifindex, 0);
         else
                 ip6_update_pmtu(skb, net, info, 0, 0);
         xfrm_state_put(x);
+
+        return 0;
 }
 
 static struct xfrm_state *ipcomp6_tunnel_create(struct xfrm_state *x)
@@ -174,6 +176,11 @@ out:
         return err;
 }
 
+static int ipcomp6_rcv_cb(struct sk_buff *skb, int err)
+{
+        return 0;
+}
+
 static const struct xfrm_type ipcomp6_type =
 {
         .description    = "IPCOMP6",
@@ -186,11 +193,12 @@ static const struct xfrm_type ipcomp6_type =
         .hdr_offset     = xfrm6_find_1stfragopt,
 };
 
-static const struct inet6_protocol ipcomp6_protocol =
+static struct xfrm6_protocol ipcomp6_protocol =
 {
         .handler        = xfrm6_rcv,
+        .cb_handler     = ipcomp6_rcv_cb,
         .err_handler    = ipcomp6_err,
-        .flags          = INET6_PROTO_NOPOLICY,
+        .priority       = 0,
 };
 
 static int __init ipcomp6_init(void)
@@ -199,7 +207,7 @@ static int __init ipcomp6_init(void)
                 pr_info("%s: can't add xfrm type\n", __func__);
                 return -EAGAIN;
         }
-        if (inet6_add_protocol(&ipcomp6_protocol, IPPROTO_COMP) < 0) {
+        if (xfrm6_protocol_register(&ipcomp6_protocol, IPPROTO_COMP) < 0) {
                 pr_info("%s: can't add protocol\n", __func__);
                 xfrm_unregister_type(&ipcomp6_type, AF_INET6);
                 return -EAGAIN;
@@ -209,7 +217,7 @@ static int __init ipcomp6_init(void)
 
 static void __exit ipcomp6_fini(void)
 {
-        if (inet6_del_protocol(&ipcomp6_protocol, IPPROTO_COMP) < 0)
+        if (xfrm6_protocol_deregister(&ipcomp6_protocol, IPPROTO_COMP) < 0)
                 pr_info("%s: can't remove protocol\n", __func__);
         if (xfrm_unregister_type(&ipcomp6_type, AF_INET6) < 0)
                 pr_info("%s: can't remove xfrm type\n", __func__);
diff --git a/net/ipv6/xfrm6_mode_tunnel.c b/net/ipv6/xfrm6_mode_tunnel.c
index cb04f7a16b5e..901ef6f8addc 100644
--- a/net/ipv6/xfrm6_mode_tunnel.c
+++ b/net/ipv6/xfrm6_mode_tunnel.c
@@ -18,65 +18,6 @@
 #include <net/ipv6.h>
 #include <net/xfrm.h>
 
-/* Informational hook. The decap is still done here. */
-static struct xfrm_tunnel_notifier __rcu *rcv_notify_handlers __read_mostly;
-static DEFINE_MUTEX(xfrm6_mode_tunnel_input_mutex);
-
-int xfrm6_mode_tunnel_input_register(struct xfrm_tunnel_notifier *handler)
-{
-        struct xfrm_tunnel_notifier __rcu **pprev;
-        struct xfrm_tunnel_notifier *t;
-        int ret = -EEXIST;
-        int priority = handler->priority;
-
-        mutex_lock(&xfrm6_mode_tunnel_input_mutex);
-
-        for (pprev = &rcv_notify_handlers;
-             (t = rcu_dereference_protected(*pprev,
-             lockdep_is_held(&xfrm6_mode_tunnel_input_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(&xfrm6_mode_tunnel_input_mutex);
-        return ret;
-}
-EXPORT_SYMBOL_GPL(xfrm6_mode_tunnel_input_register);
-
-int xfrm6_mode_tunnel_input_deregister(struct xfrm_tunnel_notifier *handler)
-{
-        struct xfrm_tunnel_notifier __rcu **pprev;
-        struct xfrm_tunnel_notifier *t;
-        int ret = -ENOENT;
-
-        mutex_lock(&xfrm6_mode_tunnel_input_mutex);
-        for (pprev = &rcv_notify_handlers;
-             (t = rcu_dereference_protected(*pprev,
-             lockdep_is_held(&xfrm6_mode_tunnel_input_mutex))) != NULL;
-             pprev = &t->next) {
-                if (t == handler) {
-                        *pprev = handler->next;
-                        ret = 0;
-                        break;
-                }
-        }
-        mutex_unlock(&xfrm6_mode_tunnel_input_mutex);
-        synchronize_net();
-
-        return ret;
-}
-EXPORT_SYMBOL_GPL(xfrm6_mode_tunnel_input_deregister);
-
 static inline void ipip6_ecn_decapsulate(struct sk_buff *skb)
 {
         const struct ipv6hdr *outer_iph = ipv6_hdr(skb);
@@ -130,7 +71,6 @@ static int xfrm6_mode_tunnel_output(struct xfrm_state *x, struct sk_buff *skb)
 
 static int xfrm6_mode_tunnel_input(struct xfrm_state *x, struct sk_buff *skb)
 {
-        struct xfrm_tunnel_notifier *handler;
         int err = -EINVAL;
 
         if (XFRM_MODE_SKB_CB(skb)->protocol != IPPROTO_IPV6)
@@ -138,9 +78,6 @@ static int xfrm6_mode_tunnel_input(struct xfrm_state *x, struct sk_buff *skb)
         if (!pskb_may_pull(skb, sizeof(struct ipv6hdr)))
                 goto out;
 
-        for_each_input_rcu(rcv_notify_handlers, handler)
-                handler->handler(skb);
-
         err = skb_unclone(skb, GFP_ATOMIC);
         if (err)
                 goto out;
diff --git a/net/ipv6/xfrm6_policy.c b/net/ipv6/xfrm6_policy.c
index 5f8e128c512d..2a0bbda2c76a 100644
--- a/net/ipv6/xfrm6_policy.c
+++ b/net/ipv6/xfrm6_policy.c
@@ -389,11 +389,17 @@ int __init xfrm6_init(void)
         if (ret)
                 goto out_policy;
 
+        ret = xfrm6_protocol_init();
+        if (ret)
+                goto out_state;
+
 #ifdef CONFIG_SYSCTL
         register_pernet_subsys(&xfrm6_net_ops);
 #endif
 out:
         return ret;
+out_state:
+        xfrm6_state_fini();
 out_policy:
         xfrm6_policy_fini();
         goto out;
@@ -404,6 +410,7 @@ void xfrm6_fini(void)
 #ifdef CONFIG_SYSCTL
         unregister_pernet_subsys(&xfrm6_net_ops);
 #endif
+        xfrm6_protocol_fini();
         xfrm6_policy_fini();
         xfrm6_state_fini();
         dst_entries_destroy(&xfrm6_dst_ops);
diff --git a/net/ipv6/xfrm6_protocol.c b/net/ipv6/xfrm6_protocol.c
new file mode 100644
index 000000000000..6ab989c486f7
--- /dev/null
+++ b/net/ipv6/xfrm6_protocol.c
@@ -0,0 +1,270 @@
+/* xfrm6_protocol.c - Generic xfrm protocol multiplexer for ipv6.
+ *
+ * Copyright (C) 2013 secunet Security Networks AG
+ *
+ * Author:
+ * Steffen Klassert <steffen.klassert@secunet.com>
+ *
+ * Based on:
+ * net/ipv4/xfrm4_protocol.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 <linux/icmpv6.h>
+#include <net/ipv6.h>
+#include <net/protocol.h>
+#include <net/xfrm.h>
+
+static struct xfrm6_protocol __rcu *esp6_handlers __read_mostly;
+static struct xfrm6_protocol __rcu *ah6_handlers __read_mostly;
+static struct xfrm6_protocol __rcu *ipcomp6_handlers __read_mostly;
+static DEFINE_MUTEX(xfrm6_protocol_mutex);
+
+static inline struct xfrm6_protocol __rcu **proto_handlers(u8 protocol)
+{
+        switch (protocol) {
+        case IPPROTO_ESP:
+                return &esp6_handlers;
+        case IPPROTO_AH:
+                return &ah6_handlers;
+        case IPPROTO_COMP:
+                return &ipcomp6_handlers;
+        }
+
+        return NULL;
+}
+
+#define for_each_protocol_rcu(head, handler)            \
+        for (handler = rcu_dereference(head);           \
+             handler != NULL;                           \
+             handler = rcu_dereference(handler->next))  \
+
+int xfrm6_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
+{
+        int ret;
+        struct xfrm6_protocol *handler;
+
+        for_each_protocol_rcu(*proto_handlers(protocol), handler)
+                if ((ret = handler->cb_handler(skb, err)) <= 0)
+                        return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL(xfrm6_rcv_cb);
+
+static int xfrm6_esp_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm6_protocol *handler;
+
+        XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
+
+        for_each_protocol_rcu(esp6_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;
+
+        icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm6_esp_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                          u8 type, u8 code, int offset, __be32 info)
+{
+        struct xfrm6_protocol *handler;
+
+        for_each_protocol_rcu(esp6_handlers, handler)
+                if (!handler->err_handler(skb, opt, type, code, offset, info))
+                        break;
+}
+
+static int xfrm6_ah_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm6_protocol *handler;
+
+        XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
+
+        for_each_protocol_rcu(ah6_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;
+
+        icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm6_ah_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                         u8 type, u8 code, int offset, __be32 info)
+{
+        struct xfrm6_protocol *handler;
+
+        for_each_protocol_rcu(ah6_handlers, handler)
+                if (!handler->err_handler(skb, opt, type, code, offset, info))
+                        break;
+}
+
+static int xfrm6_ipcomp_rcv(struct sk_buff *skb)
+{
+        int ret;
+        struct xfrm6_protocol *handler;
+
+        XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
+
+        for_each_protocol_rcu(ipcomp6_handlers, handler)
+                if ((ret = handler->handler(skb)) != -EINVAL)
+                        return ret;
+
+        icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
+
+        kfree_skb(skb);
+        return 0;
+}
+
+static void xfrm6_ipcomp_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
+                             u8 type, u8 code, int offset, __be32 info)
+{
+        struct xfrm6_protocol *handler;
+
+        for_each_protocol_rcu(ipcomp6_handlers, handler)
+                if (!handler->err_handler(skb, opt, type, code, offset, info))
+                        break;
+}
+
+static const struct inet6_protocol esp6_protocol = {
+        .handler        =       xfrm6_esp_rcv,
+        .err_handler    =       xfrm6_esp_err,
+        .flags          =       INET6_PROTO_NOPOLICY,
+};
+
+static const struct inet6_protocol ah6_protocol = {
+        .handler        =       xfrm6_ah_rcv,
+        .err_handler    =       xfrm6_ah_err,
+        .flags          =       INET6_PROTO_NOPOLICY,
+};
+
+static const struct inet6_protocol ipcomp6_protocol = {
+        .handler        =       xfrm6_ipcomp_rcv,
+        .err_handler    =       xfrm6_ipcomp_err,
+        .flags          =       INET6_PROTO_NOPOLICY,
+};
+
+static struct xfrm_input_afinfo xfrm6_input_afinfo = {
+        .family         =       AF_INET6,
+        .owner          =       THIS_MODULE,
+        .callback       =       xfrm6_rcv_cb,
+};
+
+static inline const struct inet6_protocol *netproto(unsigned char protocol)
+{
+        switch (protocol) {
+        case IPPROTO_ESP:
+                return &esp6_protocol;
+        case IPPROTO_AH:
+                return &ah6_protocol;
+        case IPPROTO_COMP:
+                return &ipcomp6_protocol;
+        }
+
+        return NULL;
+}
+
+int xfrm6_protocol_register(struct xfrm6_protocol *handler,
+                            unsigned char protocol)
+{
+        struct xfrm6_protocol __rcu **pprev;
+        struct xfrm6_protocol *t;
+        bool add_netproto = false;
+
+        int ret = -EEXIST;
+        int priority = handler->priority;
+
+        mutex_lock(&xfrm6_protocol_mutex);
+
+        if (!rcu_dereference_protected(*proto_handlers(protocol),
+                                       lockdep_is_held(&xfrm6_protocol_mutex)))
+                add_netproto = true;
+
+        for (pprev = proto_handlers(protocol);
+             (t = rcu_dereference_protected(*pprev,
+                        lockdep_is_held(&xfrm6_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(&xfrm6_protocol_mutex);
+
+        if (add_netproto) {
+                if (inet6_add_protocol(netproto(protocol), protocol)) {
+                        pr_err("%s: can't add protocol\n", __func__);
+                        ret = -EAGAIN;
+                }
+        }
+
+        return ret;
+}
+EXPORT_SYMBOL(xfrm6_protocol_register);
+
+int xfrm6_protocol_deregister(struct xfrm6_protocol *handler,
+                              unsigned char protocol)
+{
+        struct xfrm6_protocol __rcu **pprev;
+        struct xfrm6_protocol *t;
+        int ret = -ENOENT;
+
+        mutex_lock(&xfrm6_protocol_mutex);
+
+        for (pprev = proto_handlers(protocol);
+             (t = rcu_dereference_protected(*pprev,
+                        lockdep_is_held(&xfrm6_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(&xfrm6_protocol_mutex))) {
+                if (inet6_del_protocol(netproto(protocol), protocol) < 0) {
+                        pr_err("%s: can't remove protocol\n", __func__);
+                        ret = -EAGAIN;
+                }
+        }
+
+        mutex_unlock(&xfrm6_protocol_mutex);
+
+        synchronize_net();
+
+        return ret;
+}
+EXPORT_SYMBOL(xfrm6_protocol_deregister);
+
+int __init xfrm6_protocol_init(void)
+{
+        return xfrm_input_register_afinfo(&xfrm6_input_afinfo);
+}
+
+void xfrm6_protocol_fini(void)
+{
+        xfrm_input_unregister_afinfo(&xfrm6_input_afinfo);
+}
diff --git a/net/key/af_key.c b/net/key/af_key.c
index a50d979b5926..12651b42aad8 100644
--- a/net/key/af_key.c
+++ b/net/key/af_key.c
@@ -1799,7 +1799,7 @@ static void pfkey_dump_sa_done(struct pfkey_sock *pfk)
 static int pfkey_dump(struct sock *sk, struct sk_buff *skb, const struct sadb_msg *hdr, void * const *ext_hdrs)
 {
         u8 proto;
-        struct xfrm_filter *filter = NULL;
+        struct xfrm_address_filter *filter = NULL;
         struct pfkey_sock *pfk = pfkey_sk(sk);
 
         if (pfk->dump.dump != NULL)
diff --git a/net/xfrm/xfrm_input.c b/net/xfrm/xfrm_input.c
index 4218164f4f5e..85d1d4764612 100644
--- a/net/xfrm/xfrm_input.c
+++ b/net/xfrm/xfrm_input.c
@@ -16,6 +16,81 @@
 
 static struct kmem_cache *secpath_cachep __read_mostly;
 
+static DEFINE_SPINLOCK(xfrm_input_afinfo_lock);
+static struct xfrm_input_afinfo __rcu *xfrm_input_afinfo[NPROTO];
+
+int xfrm_input_register_afinfo(struct xfrm_input_afinfo *afinfo)
+{
+        int err = 0;
+
+        if (unlikely(afinfo == NULL))
+                return -EINVAL;
+        if (unlikely(afinfo->family >= NPROTO))
+                return -EAFNOSUPPORT;
+        spin_lock_bh(&xfrm_input_afinfo_lock);
+        if (unlikely(xfrm_input_afinfo[afinfo->family] != NULL))
+                err = -ENOBUFS;
+        else
+                rcu_assign_pointer(xfrm_input_afinfo[afinfo->family], afinfo);
+        spin_unlock_bh(&xfrm_input_afinfo_lock);
+        return err;
+}
+EXPORT_SYMBOL(xfrm_input_register_afinfo);
+
+int xfrm_input_unregister_afinfo(struct xfrm_input_afinfo *afinfo)
+{
+        int err = 0;
+
+        if (unlikely(afinfo == NULL))
+                return -EINVAL;
+        if (unlikely(afinfo->family >= NPROTO))
+                return -EAFNOSUPPORT;
+        spin_lock_bh(&xfrm_input_afinfo_lock);
+        if (likely(xfrm_input_afinfo[afinfo->family] != NULL)) {
+                if (unlikely(xfrm_input_afinfo[afinfo->family] != afinfo))
+                        err = -EINVAL;
+                else
+                        RCU_INIT_POINTER(xfrm_input_afinfo[afinfo->family], NULL);
+        }
+        spin_unlock_bh(&xfrm_input_afinfo_lock);
+        synchronize_rcu();
+        return err;
+}
+EXPORT_SYMBOL(xfrm_input_unregister_afinfo);
+
+static struct xfrm_input_afinfo *xfrm_input_get_afinfo(unsigned int family)
+{
+        struct xfrm_input_afinfo *afinfo;
+
+        if (unlikely(family >= NPROTO))
+                return NULL;
+        rcu_read_lock();
+        afinfo = rcu_dereference(xfrm_input_afinfo[family]);
+        if (unlikely(!afinfo))
+                rcu_read_unlock();
+        return afinfo;
+}
+
+static void xfrm_input_put_afinfo(struct xfrm_input_afinfo *afinfo)
+{
+        rcu_read_unlock();
+}
+
+static int xfrm_rcv_cb(struct sk_buff *skb, unsigned int family, u8 protocol,
+                       int err)
+{
+        int ret;
+        struct xfrm_input_afinfo *afinfo = xfrm_input_get_afinfo(family);
+
+        if (!afinfo)
+                return -EAFNOSUPPORT;
+
+        ret = afinfo->callback(skb, protocol, err);
+        xfrm_input_put_afinfo(afinfo);
+
+        return ret;
+}
+
 void __secpath_destroy(struct sec_path *sp)
 {
         int i;
diff --git a/net/xfrm/xfrm_state.c b/net/xfrm/xfrm_state.c
index 06970fee9155..8e9c781a6bba 100644
--- a/net/xfrm/xfrm_state.c
+++ b/net/xfrm/xfrm_state.c
@@ -1609,7 +1609,7 @@ unlock:
 EXPORT_SYMBOL(xfrm_alloc_spi);
 
 static bool __xfrm_state_filter_match(struct xfrm_state *x,
-                                      struct xfrm_filter *filter)
+                                      struct xfrm_address_filter *filter)
 {
         if (filter) {
                 if ((filter->family == AF_INET ||
@@ -1668,7 +1668,7 @@ out:
 EXPORT_SYMBOL(xfrm_state_walk);
 
 void xfrm_state_walk_init(struct xfrm_state_walk *walk, u8 proto,
-                          struct xfrm_filter *filter)
+                          struct xfrm_address_filter *filter)
 {
         INIT_LIST_HEAD(&walk->all);
         walk->proto = proto;
diff --git a/net/xfrm/xfrm_user.c b/net/xfrm/xfrm_user.c
index 195dbe230b98..cdd9e9c7ff0e 100644
--- a/net/xfrm/xfrm_user.c
+++ b/net/xfrm/xfrm_user.c
@@ -899,7 +899,7 @@ static int xfrm_dump_sa(struct sk_buff *skb, struct netlink_callback *cb)
 
         if (!cb->args[0]) {
                 struct nlattr *attrs[XFRMA_MAX+1];
-                struct xfrm_filter *filter = NULL;
+                struct xfrm_address_filter *filter = NULL;
                 u8 proto = 0;
                 int err;
 
@@ -910,12 +910,12 @@ static int xfrm_dump_sa(struct sk_buff *skb, struct netlink_callback *cb)
                 if (err < 0)
                         return err;
 
-                if (attrs[XFRMA_FILTER]) {
+                if (attrs[XFRMA_ADDRESS_FILTER]) {
                         filter = kmalloc(sizeof(*filter), GFP_KERNEL);
                         if (filter == NULL)
                                 return -ENOMEM;
 
-                        memcpy(filter, nla_data(attrs[XFRMA_FILTER]),
+                        memcpy(filter, nla_data(attrs[XFRMA_ADDRESS_FILTER]),
                                sizeof(*filter));
                 }
 
@@ -2329,7 +2329,7 @@ static const struct nla_policy xfrma_policy[XFRMA_MAX+1] = {
         [XFRMA_REPLAY_ESN_VAL]  = { .len = sizeof(struct xfrm_replay_state_esn) },
         [XFRMA_SA_EXTRA_FLAGS]  = { .type = NLA_U32 },
         [XFRMA_PROTO]           = { .type = NLA_U8 },
-        [XFRMA_FILTER]          = { .len = sizeof(struct xfrm_filter) },
+        [XFRMA_ADDRESS_FILTER]  = { .len = sizeof(struct xfrm_address_filter) },
 };
 
 static const struct xfrm_link {