[IPV6] MROUTE: Support PIM-SM (SSM).

Based on ancient patch by Mickael Hoerdt
<hoerdt@clarinet.u-strasbg.fr>, which is available at
<http://www-r2.u-strasbg.fr/~hoerdt/dev/linux_ipv6_mforwarding/patch-linux-ipv6-mforwarding-0.1a>.
Signed-off-by: YOSHIFUJI Hideaki <yoshfuji@linux-ipv6.org>

include/linux/mroute6.h

@@ -23,6 +23,8 @@
 #define MRT6_ADD_MFC	(MRT6_BASE+4)	/* Add a multicast forwarding entry	*/
 #define MRT6_DEL_MFC	(MRT6_BASE+5)	/* Delete a multicast forwarding entry	*/
 #define MRT6_VERSION	(MRT6_BASE+6)	/* Get the kernel multicast version	*/
+#define MRT6_ASSERT	(MRT6_BASE+7)	/* Activate PIM assert mode		*/
+#define MRT6_PIM	(MRT6_BASE+8)	/* enable PIM code	*/
 
 #define SIOCGETMIFCNT_IN6	SIOCPROTOPRIVATE	/* IP protocol privates */
 #define SIOCGETSGCNT_IN6	(SIOCPROTOPRIVATE+1)
@@ -217,6 +219,8 @@ static inline int ip6mr_sk_done(struct sock *sk) { return 0; }
 
 struct mrt6msg {
 #define MRT6MSG_NOCACHE		1
+#define MRT6MSG_WRONGMIF	2
+#define MRT6MSG_WHOLEPKT	3		/* used for use level encap */
 	__u8		im6_mbz;		/* must be zero		   */
 	__u8		im6_msgtype;		/* what type of message    */
 	__u16		im6_mif;		/* mif rec'd on		   */

net/ipv6/Kconfig

@@ -216,3 +216,10 @@ config IPV6_MROUTE
 	  Experimental support for IPv6 multicast forwarding.
 	  If unsure, say N.
 
+config IPV6_PIMSM_V2
+	bool "IPv6: PIM-SM version 2 support (EXPERIMENTAL)"
+	depends on IPV6_MROUTE
+	---help---
+	  Support for IPv6 PIM multicast routing protocol PIM-SMv2.
+	  If unsure, say N.
+

net/ipv6/ip6mr.c

@@ -54,6 +54,7 @@
 #include <net/ipv6.h>
 #include <net/ip6_route.h>
 #include <linux/mroute6.h>
+#include <linux/pim.h>
 #include <net/addrconf.h>
 #include <linux/netfilter_ipv6.h>
 
@@ -75,6 +76,13 @@ static int maxvif;
 
 #define MIF_EXISTS(idx) (vif6_table[idx].dev != NULL)
 
+static int mroute_do_assert;				/* Set in PIM assert	*/
+#ifdef CONFIG_IPV6_PIMSM_V2
+static int mroute_do_pim;
+#else
+#define mroute_do_pim 0
+#endif
+
 static struct mfc6_cache *mfc6_cache_array[MFC_LINES];	/* Forwarding cache	*/
 
 static struct mfc6_cache *mfc_unres_queue;		/* Queue of unresolved entries */
@@ -97,6 +105,10 @@ static int ip6_mr_forward(struct sk_buff *skb, struct mfc6_cache *cache);
 static int ip6mr_cache_report(struct sk_buff *pkt, vifi_t vifi, int assert);
 static int ip6mr_fill_mroute(struct sk_buff *skb, struct mfc6_cache *c, struct rtmsg *rtm);
 
+#ifdef CONFIG_IPV6_PIMSM_V2
+static struct inet6_protocol pim6_protocol;
+#endif
+
 static struct timer_list ipmr_expire_timer;
 
 
@@ -339,6 +351,132 @@ static struct file_operations ip6mr_mfc_fops = {
 };
 #endif
 
+#ifdef CONFIG_IPV6_PIMSM_V2
+static int reg_vif_num = -1;
+
+static int pim6_rcv(struct sk_buff *skb)
+{
+	struct pimreghdr *pim;
+	struct ipv6hdr   *encap;
+	struct net_device  *reg_dev = NULL;
+
+	if (!pskb_may_pull(skb, sizeof(*pim) + sizeof(*encap)))
+		goto drop;
+
+	pim = (struct pimreghdr *)skb_transport_header(skb);
+	if (pim->type != ((PIM_VERSION << 4) | PIM_REGISTER) ||
+	    (pim->flags & PIM_NULL_REGISTER) ||
+	    (ip_compute_csum((void *)pim, sizeof(*pim)) != 0 &&
+	     (u16)csum_fold(skb_checksum(skb, 0, skb->len, 0))))
+		goto drop;
+
+	/* check if the inner packet is destined to mcast group */
+	encap = (struct ipv6hdr *)(skb_transport_header(skb) +
+				   sizeof(*pim));
+
+	if (!ipv6_addr_is_multicast(&encap->daddr) ||
+	    encap->payload_len == 0 ||
+	    ntohs(encap->payload_len) + sizeof(*pim) > skb->len)
+		goto drop;
+
+	read_lock(&mrt_lock);
+	if (reg_vif_num >= 0)
+		reg_dev = vif6_table[reg_vif_num].dev;
+	if (reg_dev)
+		dev_hold(reg_dev);
+	read_unlock(&mrt_lock);
+
+	if (reg_dev == NULL)
+		goto drop;
+
+	skb->mac_header = skb->network_header;
+	skb_pull(skb, (u8 *)encap - skb->data);
+	skb_reset_network_header(skb);
+	skb->dev = reg_dev;
+	skb->protocol = htons(ETH_P_IP);
+	skb->ip_summed = 0;
+	skb->pkt_type = PACKET_HOST;
+	dst_release(skb->dst);
+	((struct net_device_stats *)netdev_priv(reg_dev))->rx_bytes += skb->len;
+	((struct net_device_stats *)netdev_priv(reg_dev))->rx_packets++;
+	skb->dst = NULL;
+	nf_reset(skb);
+	netif_rx(skb);
+	dev_put(reg_dev);
+	return 0;
+ drop:
+	kfree_skb(skb);
+	return 0;
+}
+
+static struct inet6_protocol pim6_protocol = {
+	.handler	=	pim6_rcv,
+};
+
+/* Service routines creating virtual interfaces: PIMREG */
+
+static int reg_vif_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	read_lock(&mrt_lock);
+	((struct net_device_stats *)netdev_priv(dev))->tx_bytes += skb->len;
+	((struct net_device_stats *)netdev_priv(dev))->tx_packets++;
+	ip6mr_cache_report(skb, reg_vif_num, MRT6MSG_WHOLEPKT);
+	read_unlock(&mrt_lock);
+	kfree_skb(skb);
+	return 0;
+}
+
+static struct net_device_stats *reg_vif_get_stats(struct net_device *dev)
+{
+	return (struct net_device_stats *)netdev_priv(dev);
+}
+
+static void reg_vif_setup(struct net_device *dev)
+{
+	dev->type		= ARPHRD_PIMREG;
+	dev->mtu		= 1500 - sizeof(struct ipv6hdr) - 8;
+	dev->flags		= IFF_NOARP;
+	dev->hard_start_xmit	= reg_vif_xmit;
+	dev->get_stats		= reg_vif_get_stats;
+	dev->destructor		= free_netdev;
+}
+
+static struct net_device *ip6mr_reg_vif(void)
+{
+	struct net_device *dev;
+	struct inet6_dev *in_dev;
+
+	dev = alloc_netdev(sizeof(struct net_device_stats), "pim6reg",
+			   reg_vif_setup);
+
+	if (dev == NULL)
+		return NULL;
+
+	if (register_netdevice(dev)) {
+		free_netdev(dev);
+		return NULL;
+	}
+	dev->iflink = 0;
+
+	in_dev = ipv6_find_idev(dev);
+	if (!in_dev)
+		goto failure;
+
+	if (dev_open(dev))
+		goto failure;
+
+	return dev;
+
+failure:
+	/* allow the register to be completed before unregistering. */
+	rtnl_unlock();
+	rtnl_lock();
+
+	unregister_netdevice(dev);
+	return NULL;
+}
+#endif
+
 /*
  *	Delete a VIF entry
  */
@@ -361,6 +499,11 @@ static int mif6_delete(int vifi)
 		return -EADDRNOTAVAIL;
 	}
 
+#ifdef CONFIG_IPV6_PIMSM_V2
+	if (vifi == reg_vif_num)
+		reg_vif_num = -1;
+#endif
+
 	if (vifi + 1 == maxvif) {
 		int tmp;
 		for (tmp = vifi - 1; tmp >= 0; tmp--) {
@@ -480,6 +623,19 @@ static int mif6_add(struct mif6ctl *vifc, int mrtsock)
 		return -EADDRINUSE;
 
 	switch (vifc->mif6c_flags) {
+#ifdef CONFIG_IPV6_PIMSM_V2
+	case MIFF_REGISTER:
+		/*
+		 * Special Purpose VIF in PIM
+		 * All the packets will be sent to the daemon
+		 */
+		if (reg_vif_num >= 0)
+			return -EADDRINUSE;
+		dev = ip6mr_reg_vif();
+		if (!dev)
+			return -ENOBUFS;
+		break;
+#endif
 	case 0:
 		dev = dev_get_by_index(&init_net, vifc->mif6c_pifi);
 		if (!dev)
@@ -512,6 +668,10 @@ static int mif6_add(struct mif6ctl *vifc, int mrtsock)
 	write_lock_bh(&mrt_lock);
 	dev_hold(dev);
 	v->dev = dev;
+#ifdef CONFIG_IPV6_PIMSM_V2
+	if (v->flags & MIFF_REGISTER)
+		reg_vif_num = vifi;
+#endif
 	if (vifi + 1 > maxvif)
 		maxvif = vifi + 1;
 	write_unlock_bh(&mrt_lock);
@@ -599,7 +759,13 @@ static int ip6mr_cache_report(struct sk_buff *pkt, vifi_t vifi, int assert)
 	struct mrt6msg *msg;
 	int ret;
 
-	skb = alloc_skb(sizeof(struct ipv6hdr) + sizeof(*msg), GFP_ATOMIC);
+#ifdef CONFIG_IPV6_PIMSM_V2
+	if (assert == MRT6MSG_WHOLEPKT)
+		skb = skb_realloc_headroom(pkt, -skb_network_offset(pkt)
+						+sizeof(*msg));
+	else
+#endif
+		skb = alloc_skb(sizeof(struct ipv6hdr) + sizeof(*msg), GFP_ATOMIC);
 
 	if (!skb)
 		return -ENOBUFS;
@@ -609,6 +775,29 @@ static int ip6mr_cache_report(struct sk_buff *pkt, vifi_t vifi, int assert)
 
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 
+#ifdef CONFIG_IPV6_PIMSM_V2
+	if (assert == MRT6MSG_WHOLEPKT) {
+		/* Ugly, but we have no choice with this interface.
+		   Duplicate old header, fix length etc.
+		   And all this only to mangle msg->im6_msgtype and
+		   to set msg->im6_mbz to "mbz" :-)
+		 */
+		skb_push(skb, -skb_network_offset(pkt));
+
+		skb_push(skb, sizeof(*msg));
+		skb_reset_transport_header(skb);
+		msg = (struct mrt6msg *)skb_transport_header(skb);
+		msg->im6_mbz = 0;
+		msg->im6_msgtype = MRT6MSG_WHOLEPKT;
+		msg->im6_mif = reg_vif_num;
+		msg->im6_pad = 0;
+		ipv6_addr_copy(&msg->im6_src, &ipv6_hdr(pkt)->saddr);
+		ipv6_addr_copy(&msg->im6_dst, &ipv6_hdr(pkt)->daddr);
+
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	} else
+#endif
+	{
 	/*
 	 *	Copy the IP header
 	 */
@@ -635,6 +824,7 @@ static int ip6mr_cache_report(struct sk_buff *pkt, vifi_t vifi, int assert)
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 
 	skb_pull(skb, sizeof(struct ipv6hdr));
+	}
 
 	if (mroute6_socket == NULL) {
 		kfree_skb(skb);
@@ -1033,6 +1223,44 @@ int ip6_mroute_setsockopt(struct sock *sk, int optname, char __user *optval, int
 		rtnl_unlock();
 		return ret;
 
+	/*
+	 *	Control PIM assert (to activate pim will activate assert)
+	 */
+	case MRT6_ASSERT:
+	{
+		int v;
+		if (get_user(v, (int __user *)optval))
+			return -EFAULT;
+		mroute_do_assert = !!v;
+		return 0;
+	}
+
+#ifdef CONFIG_IPV6_PIMSM_V2
+	case MRT6_PIM:
+	{
+		int v, ret;
+		if (get_user(v, (int __user *)optval))
+			return -EFAULT;
+		v = !!v;
+		rtnl_lock();
+		ret = 0;
+		if (v != mroute_do_pim) {
+			mroute_do_pim = v;
+			mroute_do_assert = v;
+			if (mroute_do_pim)
+				ret = inet6_add_protocol(&pim6_protocol,
+							 IPPROTO_PIM);
+			else
+				ret = inet6_del_protocol(&pim6_protocol,
+							 IPPROTO_PIM);
+			if (ret < 0)
+				ret = -EAGAIN;
+		}
+		rtnl_unlock();
+		return ret;
+	}
+
+#endif
 	/*
 	 *	Spurious command, or MRT_VERSION which you cannot
 	 *	set.
@@ -1056,6 +1284,14 @@ int ip6_mroute_getsockopt(struct sock *sk, int optname, char __user *optval,
 	case MRT6_VERSION:
 		val = 0x0305;
 		break;
+#ifdef CONFIG_IPV6_PIMSM_V2
+	case MRT6_PIM:
+		val = mroute_do_pim;
+		break;
+#endif
+	case MRT6_ASSERT:
+		val = mroute_do_assert;
+		break;
 	default:
 		return -ENOPROTOOPT;
 	}
@@ -1151,6 +1387,18 @@ static int ip6mr_forward2(struct sk_buff *skb, struct mfc6_cache *c, int vifi)
 	if (vif->dev == NULL)
 		goto out_free;
 
+#ifdef CONFIG_IPV6_PIMSM_V2
+	if (vif->flags & MIFF_REGISTER) {
+		vif->pkt_out++;
+		vif->bytes_out += skb->len;
+		((struct net_device_stats *)netdev_priv(vif->dev))->tx_bytes += skb->len;
+		((struct net_device_stats *)netdev_priv(vif->dev))->tx_packets++;
+		ip6mr_cache_report(skb, vifi, MRT6MSG_WHOLEPKT);
+		kfree_skb(skb);
+		return 0;
+	}
+#endif
+
 	ipv6h = ipv6_hdr(skb);
 
 	fl = (struct flowi) {
@@ -1220,6 +1468,30 @@ static int ip6_mr_forward(struct sk_buff *skb, struct mfc6_cache *cache)
 	cache->mfc_un.res.pkt++;
 	cache->mfc_un.res.bytes += skb->len;
 
+	/*
+	 * Wrong interface: drop packet and (maybe) send PIM assert.
+	 */
+	if (vif6_table[vif].dev != skb->dev) {
+		int true_vifi;
+
+		cache->mfc_un.res.wrong_if++;
+		true_vifi = ip6mr_find_vif(skb->dev);
+
+		if (true_vifi >= 0 && mroute_do_assert &&
+		    /* pimsm uses asserts, when switching from RPT to SPT,
+		       so that we cannot check that packet arrived on an oif.
+		       It is bad, but otherwise we would need to move pretty
+		       large chunk of pimd to kernel. Ough... --ANK
+		     */
+		    (mroute_do_pim || cache->mfc_un.res.ttls[true_vifi] < 255) &&
+		    time_after(jiffies,
+			       cache->mfc_un.res.last_assert + MFC_ASSERT_THRESH)) {
+			cache->mfc_un.res.last_assert = jiffies;
+			ip6mr_cache_report(skb, true_vifi, MRT6MSG_WRONGMIF);
+		}
+		goto dont_forward;
+	}
+
 	vif6_table[vif].pkt_in++;
 	vif6_table[vif].bytes_in += skb->len;
 
@@ -1241,6 +1513,7 @@ static int ip6_mr_forward(struct sk_buff *skb, struct mfc6_cache *cache)
 		return 0;
 	}
 
+dont_forward:
 	kfree_skb(skb);
 	return 0;
 }