[PATCH 03/31] net/stack: add multicast support

Jean-Baptiste Tr├ędez jean-baptiste.tredez at alstefgroup.com
Tue May 11 18:05:07 CEST 2021


From: Gilles Chanteperdrix <gilles.chanteperdrix at xenomai.org>

Signed-off-by: Philippe Gerum <rpm at xenomai.org>
---
 kernel/drivers/net/stack/include/ipv4/arp.h   |   3 +
 kernel/drivers/net/stack/include/ipv4/igmp.h  | 102 +++
 .../drivers/net/stack/include/ipv4/protocol.h |   1 +
 kernel/drivers/net/stack/include/rtdev.h      |  17 +
 .../drivers/net/stack/include/rtnet_socket.h  |   4 +
 kernel/drivers/net/stack/include/rtskb.h      |   1 +
 kernel/drivers/net/stack/ipv4/Kconfig         |   7 +
 kernel/drivers/net/stack/ipv4/Makefile        |   1 +
 kernel/drivers/net/stack/ipv4/af_inet.c       |   5 +
 kernel/drivers/net/stack/ipv4/arp.c           |  26 +
 kernel/drivers/net/stack/ipv4/igmp.c          | 616 ++++++++++++++++++
 kernel/drivers/net/stack/ipv4/ip_output.c     |  13 +-
 kernel/drivers/net/stack/ipv4/ip_sock.c       |  70 +-
 kernel/drivers/net/stack/ipv4/protocol.c      |  17 +
 kernel/drivers/net/stack/ipv4/tcp/tcp.c       |   2 +-
 kernel/drivers/net/stack/ipv4/udp/udp.c       |  41 +-
 kernel/drivers/net/stack/rtdev.c              | 202 ++++++
 17 files changed, 1117 insertions(+), 11 deletions(-)
 create mode 100644 kernel/drivers/net/stack/include/ipv4/igmp.h
 create mode 100644 kernel/drivers/net/stack/ipv4/igmp.c

diff --git a/kernel/drivers/net/stack/include/ipv4/arp.h b/kernel/drivers/net/stack/include/ipv4/arp.h
index cf037a18b..ee67478c2 100644
--- a/kernel/drivers/net/stack/include/ipv4/arp.h
+++ b/kernel/drivers/net/stack/include/ipv4/arp.h
@@ -48,4 +48,7 @@ static inline void rt_arp_solicit(struct rtnet_device *rtdev, u32 target)
 void __init rt_arp_init(void);
 void rt_arp_release(void);
 
+int rt_arp_mc_map(u32 addr, u8 *haddr,
+		  struct rtnet_device *dev, int dir);
+
 #endif /* __RTNET_ARP_H_ */
diff --git a/kernel/drivers/net/stack/include/ipv4/igmp.h b/kernel/drivers/net/stack/include/ipv4/igmp.h
new file mode 100644
index 000000000..a44e09041
--- /dev/null
+++ b/kernel/drivers/net/stack/include/ipv4/igmp.h
@@ -0,0 +1,102 @@
+/*
+ *  include/ipv4/igmp.h - Internet Group Management Protocol  [IGMP]
+ *
+ *      Adapted from linux/igmp.h to RTnet by:
+ *              Gilles Chanteperdrix <gilles.chanteperdrix at xenomai.org>
+ *	Original author:
+ *		Alan Cox <Alan.Cox at linux.org>
+ *
+ *	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.
+ */
+
+#ifndef __RTNET_IGMP_H_
+#define __RTNET_IGMP_H_
+#include <asm/byteorder.h>
+#include <linux/init.h>
+#include <linux/kconfig.h>
+#include <linux/in.h>
+#include <rtnet_socket.h>
+
+struct igmphdr {
+	__u8 type;
+	__u8 code;		/* For newer IGMP */
+	__u16 csum;
+	__u32 group;
+};
+
+#define IGMP_HOST_MEMBERSHIP_QUERY	0x11	/* From RFC1112 */
+#define IGMP_HOST_MEMBERSHIP_REPORT	0x12	/* Ditto */
+#define IGMP_DVMRP			0x13	/* DVMRP routing */
+#define IGMP_PIM			0x14	/* PIM routing */
+#define IGMP_TRACE			0x15
+#define IGMP_HOST_NEW_MEMBERSHIP_REPORT 0x16	/* New version of 0x11 */
+#define IGMP_HOST_LEAVE_MESSAGE 	0x17
+
+#define IGMP_MTRACE_RESP		0x1e
+#define IGMP_MTRACE			0x1f
+
+#define IGMP_DELAYING_MEMBER		0x01
+#define IGMP_IDLE_MEMBER		0x02
+#define IGMP_LAZY_MEMBER		0x03
+#define IGMP_SLEEPING_MEMBER		0x04
+#define IGMP_AWAKENING_MEMBER		0x05
+
+#define IGMP_MINLEN			8
+
+#define IGMP_MAX_HOST_REPORT_DELAY	10	/* max delay for response to */
+						/* query (in seconds)   */
+
+#define IGMP_TIMER_SCALE		10	/* denotes that the igmphdr->timer field */
+						/* specifies time in 10th of seconds     */
+
+#define IGMP_AGE_THRESHOLD		400	/* If this host don't hear any IGMP V1  */
+						/* message in this period of time,      */
+						/* revert to IGMP v2 router.            */
+
+#define IGMP_ALL_HOSTS		htonl(0xE0000001L)
+#define IGMP_ALL_ROUTER 	htonl(0xE0000002L)
+#define IGMP_LOCAL_GROUP	htonl(0xE0000000L)
+#define IGMP_LOCAL_GROUP_MASK	htonl(0xFFFFFF00L)
+
+enum rtip_mc_state {
+	RTIP_MC_NON_MEMBER,
+	RTIP_MC_DELAYING_MEMBER,
+	RTIP_MC_IDLE_MEMBER,
+};
+
+struct rtip_mc_socklist {
+	struct rtip_mc_socklist	*next;
+	struct ip_mreq		multi;
+};
+
+struct rtip_mc_list {
+	struct rtnet_device	*interface;
+	u32			multiaddr;
+	struct rtip_mc_list	*next;
+	int			users;
+	enum rtip_mc_state	state;
+};
+
+static inline bool rtnet_in_multicast(u32 addr)
+{
+	return IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP) &&
+		IN_MULTICAST(addr);
+}
+
+#ifdef CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP
+int rt_ip_mc_join_group(struct rtsocket *sk, struct ip_mreq *imr);
+int rt_ip_mc_leave_group(struct rtsocket *sk, struct ip_mreq *imr);
+void rt_ip_mc_drop_socket(struct rtsocket *sk);
+void rt_ip_mc_dec_group(struct rtnet_device *rtdev, u32 addr);
+void rt_ip_mc_inc_group(struct rtnet_device *rtdev, u32 addr);
+void rt_igmp_init(void);
+void rt_igmp_release(void);
+#else
+static inline void rt_igmp_init(void) { }
+static inline void rt_igmp_release(void) { }
+#endif
+
+#endif /* __RTNET_IGMP_H_ */
diff --git a/kernel/drivers/net/stack/include/ipv4/protocol.h b/kernel/drivers/net/stack/include/ipv4/protocol.h
index 1df42db67..dc31fb2cc 100644
--- a/kernel/drivers/net/stack/include/ipv4/protocol.h
+++ b/kernel/drivers/net/stack/include/ipv4/protocol.h
@@ -50,5 +50,6 @@ extern struct rtinet_protocol *rt_inet_protocols[];
 extern void rt_inet_add_protocol(struct rtinet_protocol *prot);
 extern void rt_inet_del_protocol(struct rtinet_protocol *prot);
 extern int rt_inet_socket(struct rtdm_fd *fd, int protocol);
+extern int rt_inet_socket_cleanup(struct rtdm_fd *fd);
 
 #endif /* __RTNET_PROTOCOL_H_ */
diff --git a/kernel/drivers/net/stack/include/rtdev.h b/kernel/drivers/net/stack/include/rtdev.h
index 3a9f6f8ee..e5a6bf920 100644
--- a/kernel/drivers/net/stack/include/rtdev.h
+++ b/kernel/drivers/net/stack/include/rtdev.h
@@ -64,6 +64,14 @@ enum rtnet_link_state {
 #define RTNET_LINK_STATE_PRESENT (1 << __RTNET_LINK_STATE_PRESENT)
 #define RTNET_LINK_STATE_NOCARRIER (1 << __RTNET_LINK_STATE_NOCARRIER)
 
+struct rtdev_mc_list {
+    struct rtdev_mc_list    *next;
+    __u8                    dmi_addr[MAX_ADDR_LEN];
+    unsigned char           dmi_addrlen;
+    int                     dmi_users;
+    int                     dmi_gusers;
+};
+
 /***
  *  rtnet_device
  */
@@ -114,6 +122,8 @@ struct rtnet_device {
 
 	int promiscuity;
 	int allmulti;
+	struct rtdev_mc_list *mc_list;
+	int mc_count;
 
 	__u32 local_ip; /* IP address in network order  */
 	__u32 broadcast_ip; /* broadcast IP in network order */
@@ -142,6 +152,7 @@ struct rtnet_device {
 	int (*rebuild_header)(struct rtskb *);
 	int (*hard_start_xmit)(struct rtskb *skb, struct rtnet_device *dev);
 	int (*hw_reset)(struct rtnet_device *rtdev);
+	void (*set_multicast_list) (struct rtnet_device *rtdev);
 
 	/* Transmission hook, managed by the stack core, RTcap, and RTmac
      *
@@ -244,6 +255,12 @@ void rtdev_unmap_rtskb(struct rtskb *skb);
 
 struct rtskb *rtnetdev_alloc_rtskb(struct rtnet_device *dev, unsigned int size);
 
+struct rtnet_device *rt_ip_dev_find(u32 addr);
+void rt_dev_mc_upload(struct rtnet_device *dev);
+int rt_dev_mc_delete(struct rtnet_device *dev, void *addr, int alen, int all);
+int rt_dev_mc_add(struct rtnet_device *dev, void *addr, int alen, int newonly);
+void rt_dev_set_allmulti(struct rtnet_device *dev, int inc);
+
 #define rtnetdev_priv(dev) ((dev)->priv)
 
 #define rtdev_emerg(__dev, format, args...)                                    \
diff --git a/kernel/drivers/net/stack/include/rtnet_socket.h b/kernel/drivers/net/stack/include/rtnet_socket.h
index d2caab649..af972c1a5 100644
--- a/kernel/drivers/net/stack/include/rtnet_socket.h
+++ b/kernel/drivers/net/stack/include/rtnet_socket.h
@@ -35,6 +35,8 @@
 #include <rtdm/driver.h>
 #include <stack_mgr.h>
 
+struct rtip_mc_socklist;
+
 struct rtsocket {
 	unsigned short protocol;
 
@@ -67,6 +69,8 @@ struct rtsocket {
 			int reg_index; /* index in port registry */
 			u8 tos;
 			u8 state;
+			struct rtip_mc_socklist *mc_list;
+			u32 mc_if_addr;
 		} inet;
 
 		/* packet socket specific */
diff --git a/kernel/drivers/net/stack/include/rtskb.h b/kernel/drivers/net/stack/include/rtskb.h
index 522b66715..4277fa6c6 100644
--- a/kernel/drivers/net/stack/include/rtskb.h
+++ b/kernel/drivers/net/stack/include/rtskb.h
@@ -177,6 +177,7 @@ struct rtskb {
 		struct icmphdr *icmph;
 		struct iphdr *ipihdr;
 		unsigned char *raw;
+		struct igmphdr *igmph;
 	} h;
 
 	/* network layer */
diff --git a/kernel/drivers/net/stack/ipv4/Kconfig b/kernel/drivers/net/stack/ipv4/Kconfig
index d5a6cd6d7..efe807915 100644
--- a/kernel/drivers/net/stack/ipv4/Kconfig
+++ b/kernel/drivers/net/stack/ipv4/Kconfig
@@ -62,6 +62,13 @@ config XENO_DRIVERS_NET_RTIPV4_ROUTER
 
     See Documentation/README.routing for further information.
 
+config XENO_DRIVERS_NET_RTIPV4_IGMP
+    bool "IGMP/multicast support"
+    depends on XENO_DRIVERS_NET_RTIPV4
+    default n
+    ---help---
+    Enables IGMP support of the RTnet Real-Time IPv4 protocol (multicast).
+
 config XENO_DRIVERS_NET_RTIPV4_DEBUG
     bool "RTipv4 Debugging"
     depends on XENO_DRIVERS_NET_RTIPV4
diff --git a/kernel/drivers/net/stack/ipv4/Makefile b/kernel/drivers/net/stack/ipv4/Makefile
index afdbeafce..d3a8b0c2d 100644
--- a/kernel/drivers/net/stack/ipv4/Makefile
+++ b/kernel/drivers/net/stack/ipv4/Makefile
@@ -17,3 +17,4 @@ obj-$(CONFIG_XENO_DRIVERS_NET_RTIPV4_UDP) += udp/
 obj-$(CONFIG_XENO_DRIVERS_NET_RTIPV4_TCP) += tcp/
 
 rtipv4-$(CONFIG_XENO_DRIVERS_NET_RTIPV4_ICMP) += icmp.o
+rtipv4-$(CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP) += igmp.o
diff --git a/kernel/drivers/net/stack/ipv4/af_inet.c b/kernel/drivers/net/stack/ipv4/af_inet.c
index 8bc87d90b..0081e0894 100644
--- a/kernel/drivers/net/stack/ipv4/af_inet.c
+++ b/kernel/drivers/net/stack/ipv4/af_inet.c
@@ -34,6 +34,7 @@
 #include <ipv4/ip_output.h>
 #include <ipv4/protocol.h>
 #include <ipv4/route.h>
+#include <ipv4/igmp.h>
 
 MODULE_LICENSE("GPL");
 
@@ -283,6 +284,7 @@ static int __init rt_ipv4_proto_init(void)
 		rt_inet_protocols[i] = NULL;
 
 	rt_icmp_init();
+	rt_igmp_init();
 
 #ifdef CONFIG_XENO_OPT_VFILE
 	result = xnvfile_init_dir("ipv4", &ipv4_proc_root, &rtnet_proc_root);
@@ -308,6 +310,7 @@ err2:
 err1:
 #endif /* CONFIG_XENO_OPT_VFILE */
 
+	rt_igmp_release();
 	rt_icmp_release();
 	rt_arp_release();
 	rt_ip_release();
@@ -327,6 +330,8 @@ static void __exit rt_ipv4_proto_release(void)
 	xnvfile_destroy_dir(&ipv4_proc_root);
 #endif
 
+	rt_igmp_release();
+
 	/* Transport-Layer */
 	rt_icmp_release();
 
diff --git a/kernel/drivers/net/stack/ipv4/arp.c b/kernel/drivers/net/stack/ipv4/arp.c
index 91c151e90..35c6c8f7e 100644
--- a/kernel/drivers/net/stack/ipv4/arp.c
+++ b/kernel/drivers/net/stack/ipv4/arp.c
@@ -21,6 +21,8 @@
  *
  */
 
+#include <net/ip.h>
+#include <linux/if_arp.h>
 #include <rtdev.h>
 #include <stack_mgr.h>
 #include <ipv4/arp.h>
@@ -195,6 +197,30 @@ static struct rtpacket_type arp_packet_type = {
 	handler: &rt_arp_rcv
 };
 
+/* Multicast mapping */
+
+#ifdef CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP
+
+int rt_arp_mc_map(u32 addr, u8 *haddr, struct rtnet_device *dev, int dir)
+{
+    switch (dev->type) {
+    case ARPHRD_ETHER:
+    case ARPHRD_FDDI:
+    case ARPHRD_IEEE802:
+        ip_eth_mc_map(addr, haddr);
+        return 0;
+    default:
+        if (dir) {
+            memcpy(haddr, dev->broadcast, dev->addr_len);
+            return 0;
+        }
+    }
+    return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(rt_arp_mc_map);
+
+#endif
+
 /***
  *  rt_arp_init
  */
diff --git a/kernel/drivers/net/stack/ipv4/igmp.c b/kernel/drivers/net/stack/ipv4/igmp.c
new file mode 100644
index 000000000..3da21df5b
--- /dev/null
+++ b/kernel/drivers/net/stack/ipv4/igmp.c
@@ -0,0 +1,616 @@
+/*
+ *      ipv4/igmp.c - Internet Group Management Protocol  [IGMP]
+ *
+ *      Adapted from net/ipv4/igmp.c to RTnet by:
+ *              Gilles Chanteperdrix <gilles.chanteperdrix at xenomai.org>
+ *	Original author:
+ *		Alan Cox <Alan.Cox at linux.org>
+ *
+ *	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.
+ *
+ *	Fixes:
+ *
+ *		Alan Cox	:	Added lots of __inline__ to optimise
+ *					the memory usage of all the tiny little
+ *					functions.
+ *		Alan Cox	:	Dumped the header building experiment.
+ *		Alan Cox	:	Minor tweaks ready for multicast routing
+ *					and extended IGMP protocol.
+ *		Alan Cox	:	Removed a load of inline directives. Gcc 2.5.8
+ *					writes utterly bogus code otherwise (sigh)
+ *					fixed IGMP loopback to behave in the manner
+ *					desired by mrouted, fixed the fact it has been
+ *					broken since 1.3.6 and cleaned up a few minor
+ *					points.
+ *
+ *		Chih-Jen Chang	:	Tried to revise IGMP to Version 2
+ *		Tsu-Sheng Tsao		E-mail: chihjenc at scf.usc.edu and tsusheng at scf.usc.edu
+ *					The enhancements are mainly based on Steve Deering's
+ *					ipmulti-3.5 source code.
+ *		Chih-Jen Chang	:	Added the igmp_get_mrouter_info and
+ *		Tsu-Sheng Tsao		igmp_set_mrouter_info to keep track of
+ *					the mrouted version on that device.
+ *		Chih-Jen Chang	:	Added the max_resp_time parameter to
+ *		Tsu-Sheng Tsao		igmp_heard_query(). Using this parameter
+ *					to identify the multicast router version
+ *					and do what the IGMP version 2 specified.
+ *		Chih-Jen Chang	:	Added a timer to revert to IGMP V2 router
+ *		Tsu-Sheng Tsao		if the specified time expired.
+ *		Alan Cox	:	Stop IGMP from 0.0.0.0 being accepted.
+ *		Alan Cox	:	Use GFP_ATOMIC in the right places.
+ *		Christian Daudt :	igmp timer wasn't set for local group
+ *					memberships but was being deleted,
+ *					which caused a "del_timer() called
+ *					from %p with timer not initialized\n"
+ *					message (960131).
+ *		Christian Daudt :	removed del_timer from
+ *					igmp_timer_expire function (960205).
+ *             Christian Daudt :       igmp_heard_report now only calls
+ *                                     igmp_timer_expire if tm->running is
+ *                                     true (960216).
+ *		Malcolm Beattie :	ttl comparison wrong in igmp_rcv made
+ *					igmp_heard_query never trigger. Expiry
+ *					miscalculation fixed in igmp_heard_query
+ *					and random() made to return unsigned to
+ *					prevent negative expiry times.
+ *		Alexey Kuznetsov:	Wrong group leaving behaviour, backport
+ *					fix from pending 2.1.x patches.
+ *		Alan Cox:		Forget to enable FDDI support earlier.
+ *		Alexey Kuznetsov:	Fixed leaving groups on device down.
+ *		Alexey Kuznetsov:	Accordance to igmp-v2-06 draft.
+ */
+
+#include <ipv4/igmp.h>
+#include <net/checksum.h>
+#include <net/ip.h>
+#include <rtnet_socket.h>
+#include <rtdev.h>
+#include <ipv4/protocol.h>
+#include <ipv4/route.h>
+#include <ipv4/arp.h>
+
+#define IGMP_REPLY_POOL_SIZE  8
+#define IP_MAX_MEMBERSHIPS 20
+#define RT_IGMP_SKB_PRIO     RTSKB_PRIO_VALUE(QUEUE_MIN_PRIO-1,		\
+						RTSKB_DEF_NRT_CHANNEL)
+/***
+ *  It is not part of the socket pool. It may furthermore be used concurrently
+ *  by multiple tasks because all fields are static excect skb_pool, but that
+ *  is spin lock protected.
+ */
+static struct rtsocket igmp_socket;
+static struct rtip_mc_list *mc_list;
+static rtdm_lock_t  mc_list_lock;
+static rtdm_mutex_t mc_socklist_lock;
+rtdm_task_t rtnet_igmp_task;
+
+#define IGMP_SIZE (sizeof(struct igmphdr)+sizeof(struct iphdr)+4)
+
+static int rt_igmp_send_report(struct rtnet_device *rtdev, u32 group, int type)
+{
+	struct rtskb *skb;
+	struct iphdr *iph;
+	struct igmphdr *ih;
+	struct dest_route rt;
+	u32 dst;
+	int len;
+	int err;
+	char buf[MAX_ADDR_LEN];
+	/* According to IGMPv2 specs, LEAVE messages are
+	 * sent to all-routers group.
+	 */
+	dst = group;
+	if (type == IGMP_HOST_LEAVE_MESSAGE)
+		dst = IGMP_ALL_ROUTER;
+	if (rt_arp_mc_map(dst, buf, rtdev, 0) == 0) {
+		memcpy(rt.dev_addr, buf, sizeof(buf));
+		rt.rtdev = rtdev;
+		rt.ip = dst;
+	}
+
+	len = (rtdev->hard_header_len + 15) & ~15;
+	skb = alloc_rtskb(len + IGMP_SIZE + 15, &global_pool);
+	if (skb == NULL) {
+		printk("can't alloc rtskb \n");
+		return -ENOMEM;
+	}
+
+	skb->rtdev = rtdev;
+	skb->priority = RT_IGMP_SKB_PRIO;
+	rtskb_reserve(skb, len);
+	skb->nh.iph = iph = (struct iphdr *)rtskb_put(skb, sizeof(*iph) + 4);
+	iph->version = 4;
+	iph->ihl = (sizeof(struct iphdr) + 4) >> 2;
+	iph->tos = 0xc0;
+	iph->frag_off = htons(IP_DF);
+	iph->ttl = 1;
+	iph->daddr = rt.ip;
+	iph->saddr = rtdev->local_ip;
+	iph->protocol = IPPROTO_IGMP;
+	iph->tot_len = htons(IGMP_SIZE);
+	iph->id = htons(0);
+	((u8 *) & iph[1])[0] = IPOPT_RA;
+	((u8 *) & iph[1])[1] = 4;
+	((u8 *) & iph[1])[2] = 0;
+	((u8 *) & iph[1])[3] = 0;
+	ip_send_check(iph);
+	ih = (struct igmphdr *)rtskb_put(skb, sizeof(struct igmphdr));
+	ih->type = type;
+	ih->code = 0;
+	ih->csum = 0;
+	ih->group = group;
+	ih->csum = ip_compute_csum((void *)ih, sizeof(struct igmphdr));
+
+	if (rtdev->hard_header) {
+		err = rtdev->hard_header(skb, rtdev, ETH_P_IP, rt.dev_addr,
+					 rtdev->dev_addr, skb->len);
+		if (err < 0) {
+		    kfree_rtskb(skb);
+		    return err;
+		}
+	}
+
+	return rtdev_xmit(skb);
+}
+
+static void igmp_heard_query(struct rtnet_device *rtdev, u32 group)
+{
+    struct rtip_mc_list	*im;
+    unsigned long     flags;
+
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (im = mc_list; im != NULL; im = im->next)
+	if (im->multiaddr != IGMP_ALL_HOSTS
+	    && (!group || im->multiaddr == group))
+	    im->state = RTIP_MC_DELAYING_MEMBER;
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+}
+
+static void igmp_heard_report(struct rtnet_device *rtdev, u32 group)
+{
+    struct rtip_mc_list	*im;
+    unsigned long     flags;
+
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (im = mc_list; im != NULL; im = im->next)
+	if (im->multiaddr == group
+	    && im->state == RTIP_MC_DELAYING_MEMBER)
+	    im->state = RTIP_MC_IDLE_MEMBER;
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+}
+
+void rt_igmp_rcv(struct rtskb *skb)
+{
+	/* This basically follows the spec line by line -- see RFC1112 */
+	struct igmphdr *ih = skb->h.igmph;
+	struct rtnet_device *rtdev = skb->rtdev;
+	int len = skb->len;
+	if (len < sizeof(struct igmphdr) || ip_compute_csum((void *)ih, len))
+		goto cleanup;
+
+	switch (ih->type) {
+	case IGMP_HOST_MEMBERSHIP_QUERY:
+		igmp_heard_query(rtdev, ih->group);
+		break;
+	case IGMP_HOST_MEMBERSHIP_REPORT:
+	case IGMP_HOST_NEW_MEMBERSHIP_REPORT:
+		igmp_heard_report(rtdev, ih->group);
+		break;
+	case IGMP_PIM:
+	case IGMP_DVMRP:
+	case IGMP_TRACE:
+	case IGMP_HOST_LEAVE_MESSAGE:
+	case IGMP_MTRACE:
+	case IGMP_MTRACE_RESP:
+		break;
+	default:
+		rtdm_printk(KERN_DEBUG
+			    "New IGMP type=0x%x, why we do not know about it?\n",
+			    ih->type);
+	}
+
+cleanup:
+	kfree_rtskb(skb);
+}
+
+/*
+ *	Add a filter to a device
+ */
+static void rt_ip_mc_filter_add(struct rtnet_device *rtdev, u32 addr)
+{
+	char buf[MAX_ADDR_LEN];
+
+	/* Checking for IFF_MULTICAST here is WRONG-WRONG-WRONG.
+	   We will get multicast token leakage, when IFF_MULTICAST
+	   is changed. This check should be done in dev->set_multicast_list
+	   routine. Something sort of:
+	   if (dev->mc_list && dev->flags&IFF_MULTICAST) { do it; }
+	   --ANK
+	 */
+	if (rt_arp_mc_map(addr, buf, rtdev, 0) == 0)
+		rt_dev_mc_add(rtdev, buf, rtdev->addr_len, 0);
+}
+
+/*
+ *	Remove a filter from a device
+ */
+static void rt_ip_mc_filter_del(struct rtnet_device *rtdev, u32 addr)
+{
+	char buf[MAX_ADDR_LEN];
+	struct rtnet_device *dev = rtdev;
+
+	if (rt_arp_mc_map(addr, buf, dev, 0) == 0)
+		rt_dev_mc_delete(dev, buf, dev->addr_len, 0);
+}
+
+static void igmp_group_dropped(struct rtip_mc_list *im)
+{
+    unsigned long flags;
+    enum rtip_mc_state oldstate;
+
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    oldstate = im->state;
+    if (oldstate != RTIP_MC_NON_MEMBER)
+	im->state = RTIP_MC_NON_MEMBER;
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+    if (oldstate != RTIP_MC_NON_MEMBER)
+	rt_ip_mc_filter_del(im->interface, im->multiaddr);
+}
+
+static void igmp_group_added(struct rtip_mc_list *im)
+{
+    unsigned long flags;
+    enum rtip_mc_state oldstate;
+
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    oldstate = im->state;
+    if (oldstate == RTIP_MC_NON_MEMBER) {
+	if (im->multiaddr == IGMP_ALL_HOSTS)
+	    im->state = RTIP_MC_IDLE_MEMBER;
+	else
+	    im->state = RTIP_MC_DELAYING_MEMBER;
+    }
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+    if (oldstate != RTIP_MC_NON_MEMBER) {
+	return;
+    }
+
+    rt_ip_mc_filter_add(im->interface, im->multiaddr);
+
+    if (im->multiaddr != IGMP_ALL_HOSTS && rtdm_in_rt_context())
+	rt_igmp_send_report(im->interface,
+			    im->multiaddr, IGMP_HOST_MEMBERSHIP_REPORT);
+}
+
+void rt_ip_mc_inc_group(struct rtnet_device *rtdev, u32 addr)
+{
+    struct rtip_mc_list *im, *iml;
+    unsigned long flags;
+
+    iml = rtdm_malloc(sizeof(*im));
+    if (!iml)
+	return;
+    iml->users = 1;
+    iml->interface = rtdev;
+    iml->multiaddr = addr;
+    iml->state = RTIP_MC_NON_MEMBER;
+
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (im = mc_list; im; im = im->next) {
+	if (im->multiaddr == addr && im->interface == rtdev) {
+	    im->users++;
+	    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+	    rtdm_free(iml);
+	    return;
+	}
+    }
+    iml->next = mc_list;
+    mc_list = iml;
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+    igmp_group_added(iml);
+}
+
+/*
+ *	A socket has left a multicast group on device dev
+ */
+void rt_ip_mc_dec_group(struct rtnet_device *rtdev, u32 addr)
+{
+    struct rtip_mc_list *i, **ip;
+    unsigned long	flags;
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (ip = &mc_list; (i = *ip) != NULL; ip = &i->next) {
+	if (i->multiaddr == addr && i->interface == rtdev) {
+	    if (--i->users == 0) {
+		*ip = i->next;
+		rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+		igmp_group_dropped(i);
+		rtdm_free(i);
+		return;
+	    }
+	    break;
+	}
+    }
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+}
+
+static struct rtnet_device *rt_ip_mc_find_dev(struct ip_mreq *imr)
+{
+    struct rtnet_device *rtdev = NULL;
+
+    if (imr->imr_interface.s_addr)
+	rtdev = rt_ip_dev_find(imr->imr_interface.s_addr);
+
+    return rtdev;
+}
+
+/*
+ *	Join a socket to a group
+ */
+
+int rt_ip_mc_join_group(struct rtsocket *sk, struct ip_mreq *imr)
+{
+    int err = 0;
+    u32 addr = imr->imr_multiaddr.s_addr;
+    struct rtnet_device *rtdev = rt_ip_mc_find_dev(imr);
+    struct rtip_mc_socklist *i, *iml;
+
+    if (!rtdev)
+	return -ENODEV;
+
+    if (!IN_MULTICAST(ntohl(addr))) {
+	err = -EINVAL;
+	goto done;
+    }
+
+    iml = rtdm_malloc(sizeof(*iml));
+    if (!iml) {
+	err = -ENOMEM;
+	goto done;
+    }
+    iml->multi = *imr;
+
+    rtdm_mutex_lock(&mc_socklist_lock);
+    for (i = sk->prot.inet.mc_list; i; i = i->next)
+	    if (i->multi.imr_multiaddr.s_addr == addr &&
+		i->multi.imr_interface.s_addr == imr->imr_interface.s_addr) {
+		rtdm_mutex_unlock(&mc_socklist_lock);
+		rtdm_free(iml);
+		err = 0;
+		goto done;
+	    }
+
+    iml->next = sk->prot.inet.mc_list;
+    sk->prot.inet.mc_list = iml;
+    rtdm_mutex_unlock(&mc_socklist_lock);
+
+    rt_ip_mc_inc_group(rtdev, addr);
+
+  done:
+    rtdev_dereference(rtdev);
+    return err;
+}
+
+/*
+ *	Ask a socket to leave a group.
+ */
+int rt_ip_mc_leave_group(struct rtsocket *sk, struct ip_mreq *imr)
+{
+    u32 addr = imr->imr_multiaddr.s_addr;
+    struct  rtnet_device *rtdev = rt_ip_mc_find_dev(imr);
+    struct rtip_mc_socklist *i, **ip;
+
+    if (!rtdev)
+	return -ENODEV;
+
+    rtdm_mutex_lock(&mc_socklist_lock);
+    for (ip = &sk->prot.inet.mc_list; (i = *ip); ip = &i->next)
+	    if (i->multi.imr_multiaddr.s_addr == addr &&
+		i->multi.imr_interface.s_addr == imr->imr_interface.s_addr) {
+		*ip = i->next;
+		rtdm_mutex_unlock(&mc_socklist_lock);
+		goto found;
+	    }
+    rtdm_mutex_unlock(&mc_socklist_lock);
+
+    rtdev_dereference(rtdev);
+
+    return -EADDRNOTAVAIL;
+
+  found:
+    rt_igmp_send_report(rtdev, addr, IGMP_HOST_LEAVE_MESSAGE);
+    rt_ip_mc_dec_group(rtdev, addr);
+    rtdev_dereference(rtdev);
+
+    rtdm_free(i);
+
+    return 0;
+}
+
+/*
+ *	A socket is closing.
+ */
+void rt_ip_mc_drop_socket(struct rtsocket *sk)
+{
+    struct rtip_mc_socklist *i, *in;
+
+    if (sk->prot.inet.mc_list == NULL)
+	return;
+
+    for (i = sk->prot.inet.mc_list; i; i = in) {
+	struct rtnet_device *rtdev;
+
+	in = i->next;
+	rtdev = rt_ip_mc_find_dev(&i->multi);
+	if (!rtdev)
+	    continue;
+
+	rt_ip_mc_dec_group(rtdev, i->multi.imr_multiaddr.s_addr);
+	rtdev_dereference(rtdev);
+    }
+}
+
+static void process(void *arg)
+{
+    struct rtip_mc_list *im;
+    unsigned long flags;
+
+    while(!rtdm_task_should_stop()){
+	rtdm_lock_get_irqsave(&mc_list_lock, flags);
+	for (im = mc_list; im; im = im->next) {
+	    if (im->state == RTIP_MC_DELAYING_MEMBER) {
+		im->state = RTIP_MC_IDLE_MEMBER;
+		im->users++;
+		rtdev_reference(im->interface);
+		break;
+	    }
+	}
+	rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+	if (im) {
+	    rt_igmp_send_report(im->interface,
+				im->multiaddr, IGMP_HOST_MEMBERSHIP_REPORT);
+	    rt_ip_mc_dec_group(im->interface, im->multiaddr);
+	    rtdev_dereference(im->interface);
+	}
+
+	rtdm_task_wait_period(NULL);
+    }
+}
+
+static int rt_igmp_socket(struct rtdm_fd *fd)
+{
+	/* we don't support user-created ICMP sockets */
+	return -ENOPROTOOPT;
+}
+
+static struct rtsocket *rt_igmp_dest_socket(struct rtskb *skb)
+{
+	/* Note that the socket's refcount is not used by this protocol.
+	 * The socket returned here is static and not part of the global pool. */
+	return &igmp_socket;
+}
+
+void rt_igmp_rcv_err(struct rtskb *skb)
+{
+	rtdm_printk("RTnet: rt_igmp_rcv err\n");
+}
+
+static struct rtinet_protocol igmp_protocol = {
+	.protocol = IPPROTO_IGMP,
+	.dest_socket = rt_igmp_dest_socket,
+	.rcv_handler = rt_igmp_rcv,
+	.err_handler = rt_igmp_rcv_err,
+	.init_socket = rt_igmp_socket
+};
+
+static void rt_ip_mc_unregister(struct rtnet_device *rtdev)
+{
+    struct rtip_mc_list *i, **ip;
+    unsigned long flags;
+
+    if (rtdev->flags & IFF_LOOPBACK)
+	return;
+
+  restart:
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (ip = &mc_list; (i = *ip) != NULL; ip = &i->next) {
+	if (i->interface != rtdev)
+	    continue;
+	*ip = i->next;
+	rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+	igmp_group_dropped(i);
+	rtdm_free(i);
+	goto restart;
+    }
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+}
+
+static void rt_ip_mc_down(struct rtnet_device *rtdev)
+{
+    struct rtip_mc_list *i;
+    unsigned long flags;
+
+    if (rtdev->flags & IFF_LOOPBACK)
+	return;
+
+restart:
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (i = mc_list; i; i = i->next) {
+	if (i->interface != rtdev || i->state == RTIP_MC_NON_MEMBER)
+	    continue;
+	rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+	igmp_group_dropped(i);
+	goto restart;
+    }
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+    rt_ip_mc_dec_group(rtdev, IGMP_ALL_HOSTS);
+}
+
+static void rt_ip_mc_up(struct rtnet_device *rtdev,
+			struct rtnet_core_cmd *up_cmd)
+{
+    struct rtip_mc_list *i;
+    unsigned long flags;
+
+    if (rtdev->flags & IFF_LOOPBACK)
+	return;
+
+    rt_ip_mc_inc_group(rtdev, IGMP_ALL_HOSTS);
+
+restart:
+    rtdm_lock_get_irqsave(&mc_list_lock, flags);
+    for (i = mc_list; i; i = i->next) {
+	if (i->interface != rtdev || i->state != RTIP_MC_NON_MEMBER)
+	    continue;
+	rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+
+	igmp_group_added(i);
+	goto restart;
+    }
+    rtdm_lock_put_irqrestore(&mc_list_lock, flags);
+}
+
+static struct rtdev_event_hook rtdev_hook = {
+	.unregister_device = rt_ip_mc_unregister,
+	.ifup = rt_ip_mc_up,
+	.ifdown = rt_ip_mc_down,
+};
+
+void __init rt_igmp_init(void)
+{
+	unsigned int skbs;
+
+	igmp_socket.protocol = IPPROTO_IGMP;
+	igmp_socket.prot.inet.tos = 0;
+	igmp_socket.priority = 0;
+	rtdm_lock_init(&mc_list_lock);
+	rtdm_mutex_init(&mc_socklist_lock);
+	/* create the rtskb pool */
+	skbs = rtskb_pool_init(&igmp_socket.skb_pool, IGMP_REPLY_POOL_SIZE,
+			NULL, NULL);
+	if (skbs < IGMP_REPLY_POOL_SIZE)
+		printk("RTnet: allocated only %d igmp rtskbs\n", skbs);
+
+	rt_inet_add_protocol(&igmp_protocol);
+	rtdm_task_init(&rtnet_igmp_task, "igmp", process, 0,
+		       RTDM_TASK_LOWEST_PRIORITY, 10000000);
+	rtdev_add_event_hook(&rtdev_hook);
+}
+
+void rt_igmp_release(void)
+{
+	rtdev_del_event_hook(&rtdev_hook);
+	rtdm_task_destroy(&rtnet_igmp_task);
+	rt_inet_del_protocol(&igmp_protocol);
+	rtskb_pool_release(&igmp_socket.skb_pool);
+	rtdm_mutex_destroy(&mc_socklist_lock);
+}
diff --git a/kernel/drivers/net/stack/ipv4/ip_output.c b/kernel/drivers/net/stack/ipv4/ip_output.c
index 664d0c790..e1bba9213 100644
--- a/kernel/drivers/net/stack/ipv4/ip_output.c
+++ b/kernel/drivers/net/stack/ipv4/ip_output.c
@@ -30,10 +30,19 @@
 #include <ipv4/ip_fragment.h>
 #include <ipv4/ip_input.h>
 #include <ipv4/route.h>
+#include <ipv4/igmp.h>
 
 static DEFINE_RTDM_LOCK(rt_ip_id_lock);
 static u16 rt_ip_id_count = 0;
 
+static inline u8 get_ttl(struct dest_route *rt)
+{
+	if (rtnet_in_multicast(ntohl(rt->ip)))
+		return 1;
+
+	return 255;
+}
+
 /***
  *  Slow path for fragmented packets
  */
@@ -108,7 +117,7 @@ int rt_ip_build_xmit_slow(struct rtsocket *sk,
 		iph->tot_len = htons(fraglen);
 		iph->id = htons(msg_rt_ip_id);
 		iph->frag_off = htons(frag_off);
-		iph->ttl = 255;
+		iph->ttl = get_ttl(rt);
 		iph->protocol = sk->protocol;
 		iph->saddr = rtdev->local_ip;
 		iph->daddr = rt->ip;
@@ -210,7 +219,7 @@ int rt_ip_build_xmit(struct rtsocket *sk,
 	iph->tot_len = htons(length);
 	iph->id = htons(msg_rt_ip_id);
 	iph->frag_off = htons(IP_DF);
-	iph->ttl = 255;
+	iph->ttl = get_ttl(rt);
 	iph->protocol = sk->protocol;
 	iph->saddr = rtdev->local_ip;
 	iph->daddr = rt->ip;
diff --git a/kernel/drivers/net/stack/ipv4/ip_sock.c b/kernel/drivers/net/stack/ipv4/ip_sock.c
index 8ca6aebd1..0349ed1b1 100644
--- a/kernel/drivers/net/stack/ipv4/ip_sock.c
+++ b/kernel/drivers/net/stack/ipv4/ip_sock.c
@@ -27,9 +27,10 @@
 #include <linux/in.h>
 
 #include <rtnet_socket.h>
+#include <ipv4/igmp.h>
 
 int rt_ip_setsockopt(struct rtdm_fd *fd, struct rtsocket *s, int level,
-		     int optname, const void __user *optval, socklen_t optlen)
+		     int optname, const void __user *u_optval, socklen_t optlen)
 {
 	int err = 0;
 	unsigned int _tos, *tos;
@@ -37,18 +38,77 @@ int rt_ip_setsockopt(struct rtdm_fd *fd, struct rtsocket *s, int level,
 	if (level != SOL_IP)
 		return -ENOPROTOOPT;
 
-	if (optlen < sizeof(unsigned int))
-		return -EINVAL;
-
 	switch (optname) {
 	case IP_TOS:
-		tos = rtnet_get_arg(fd, &_tos, optval, sizeof(_tos));
+		if (optlen < sizeof(unsigned int))
+			return -EINVAL;
+		tos = rtnet_get_arg(fd, &_tos, u_optval, sizeof(_tos));
 		if (IS_ERR(tos))
 			return PTR_ERR(tos);
 		else
 			s->prot.inet.tos = *tos;
 		break;
 
+#ifdef CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP
+	case IP_ADD_MEMBERSHIP: {
+		const struct ip_mreq *mreq;
+		struct ip_mreq _mreq;
+
+		if (optlen < sizeof(*mreq))
+			return -EINVAL;
+
+		if (!rtdm_in_rt_context())
+			return -ENOSYS;
+
+		mreq = rtnet_get_arg(fd, &_mreq, u_optval, sizeof(_mreq));
+		if (IS_ERR(mreq))
+			return PTR_ERR(mreq);
+
+		err = rt_ip_mc_join_group(s, mreq);
+		break;
+	}
+
+	case IP_DROP_MEMBERSHIP: {
+		const struct ip_mreq *mreq;
+		struct ip_mreq _mreq;
+
+		if (optlen < sizeof(*mreq))
+			return -EINVAL;
+
+		if (!rtdm_in_rt_context())
+			return -ENOSYS;
+
+		mreq = rtnet_get_arg(fd, &_mreq, u_optval, sizeof(_mreq));
+		if (IS_ERR(mreq))
+			return PTR_ERR(mreq);
+
+		err = rt_ip_mc_leave_group(s, mreq);
+		break;
+	}
+
+	case IP_MULTICAST_IF: {
+		if (optlen < sizeof(struct in_addr))
+			return -EINVAL;
+
+		if (optlen >= sizeof(struct ip_mreq)) {
+			const struct ip_mreq *mreq;
+			struct ip_mreq _mreq;
+			mreq = rtnet_get_arg(fd, &_mreq, u_optval, sizeof(_mreq));
+			if (IS_ERR(mreq))
+				return PTR_ERR(mreq);
+			s->prot.inet.mc_if_addr = mreq->imr_interface.s_addr;
+		} else {
+			const struct in_addr *in_addr;
+			struct in_addr _in_addr;
+			in_addr = rtnet_get_arg(fd, &_in_addr, u_optval, sizeof(_in_addr));
+			if (IS_ERR(in_addr))
+				return PTR_ERR(in_addr);
+			s->prot.inet.mc_if_addr = in_addr->s_addr;
+		}
+
+		break;
+	}
+#endif
 	default:
 		err = -ENOPROTOOPT;
 		break;
diff --git a/kernel/drivers/net/stack/ipv4/protocol.c b/kernel/drivers/net/stack/ipv4/protocol.c
index f56177da8..6036d6d74 100644
--- a/kernel/drivers/net/stack/ipv4/protocol.c
+++ b/kernel/drivers/net/stack/ipv4/protocol.c
@@ -28,6 +28,7 @@
 
 #include <rtnet_socket.h>
 #include <ipv4/protocol.h>
+#include <ipv4/igmp.h>
 
 struct rtinet_protocol *rt_inet_protocols[MAX_RT_INET_PROTOCOLS];
 
@@ -62,6 +63,7 @@ EXPORT_SYMBOL_GPL(rt_inet_del_protocol);
  */
 int rt_inet_socket(struct rtdm_fd *fd, int protocol)
 {
+        struct rtsocket *sock = rtdm_fd_to_private(fd);
 	struct rtinet_protocol *prot;
 
 	if (protocol == 0)
@@ -74,6 +76,8 @@ int rt_inet_socket(struct rtdm_fd *fd, int protocol)
 			break;
 		}
 
+	sock->prot.inet.mc_list = NULL;
+	sock->prot.inet.mc_if_addr = INADDR_ANY;
 	prot = rt_inet_protocols[rt_inet_hashkey(protocol)];
 
 	/* create the socket (call the socket creator) */
@@ -86,3 +90,16 @@ int rt_inet_socket(struct rtdm_fd *fd, int protocol)
 	}
 }
 EXPORT_SYMBOL_GPL(rt_inet_socket);
+
+int rt_inet_socket_cleanup(struct rtdm_fd *fd)
+{
+#ifdef CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP
+    struct rtsocket *sock = rtdm_fd_to_private(fd);
+    rt_ip_mc_drop_socket(sock);
+#endif
+
+    rt_socket_cleanup(fd);
+
+    return 0;
+}
+EXPORT_SYMBOL(rt_inet_socket_cleanup);
diff --git a/kernel/drivers/net/stack/ipv4/tcp/tcp.c b/kernel/drivers/net/stack/ipv4/tcp/tcp.c
index 71628ba03..79c25271c 100644
--- a/kernel/drivers/net/stack/ipv4/tcp/tcp.c
+++ b/kernel/drivers/net/stack/ipv4/tcp/tcp.c
@@ -1442,7 +1442,7 @@ static void rt_tcp_close(struct rtdm_fd *fd)
 
 	rt_tcp_socket_destruct(ts);
 
-	rt_socket_cleanup(fd);
+	rt_inet_socket_cleanup(fd);
 }
 
 /***
diff --git a/kernel/drivers/net/stack/ipv4/udp/udp.c b/kernel/drivers/net/stack/ipv4/udp/udp.c
index 6fe1aeb12..dcaec5618 100644
--- a/kernel/drivers/net/stack/ipv4/udp/udp.c
+++ b/kernel/drivers/net/stack/ipv4/udp/udp.c
@@ -43,6 +43,8 @@
 #include <ipv4/protocol.h>
 #include <ipv4/route.h>
 #include <ipv4/udp.h>
+#include <ipv4/arp.h>
+#include <ipv4/igmp.h>
 
 /***
  *  This structure is used to register a UDP socket for reception. All
@@ -349,7 +351,7 @@ void rt_udp_close(struct rtdm_fd *fd)
 	while ((del = rtskb_dequeue(&sock->incoming)) != NULL)
 		kfree_rtskb(del);
 
-	rt_socket_cleanup(fd);
+	rt_inet_socket_cleanup(fd);
 }
 
 int rt_udp_ioctl(struct rtdm_fd *fd, unsigned int request, void __user *arg)
@@ -567,6 +569,36 @@ static int rt_udp_getfrag(const void *p, unsigned char *to, unsigned int offset,
 	return 0;
 }
 
+static int route_multicast(struct dest_route *rt,
+			   struct rtsocket *sock,
+			   u32 daddr, u32 *saddr)
+{
+	char buf[MAX_ADDR_LEN];
+
+	if (!rtnet_in_multicast(ntohl(daddr)))
+		return -ENOTSUPP;
+
+	if (sock->prot.inet.mc_if_addr != INADDR_ANY)
+	    *saddr = sock->prot.inet.mc_if_addr;
+	else if (*saddr == INADDR_ANY)
+		return -EHOSTUNREACH;
+
+	rt->rtdev = rt_ip_dev_find(*saddr);
+	if (rt->rtdev == NULL)
+		return -EHOSTUNREACH;
+
+	if (rt_arp_mc_map(daddr, buf, rt->rtdev, 0)) {
+		rtdev_dereference(rt->rtdev);
+		rtdm_printk("can't map multicast adress %08x \n", daddr);
+		return -EHOSTUNREACH;
+	}
+
+	memcpy(rt->dev_addr, buf, sizeof(buf));
+	rt->ip = daddr;
+
+	return 0;
+}
+
 /***
  *  rt_udp_sendmsg
  */
@@ -652,6 +684,8 @@ ssize_t rt_udp_sendmsg(struct rtdm_fd *fd, const struct user_msghdr *msg,
 	}
 
 	/* get output route */
+	err = route_multicast(&rt, sock, daddr, &saddr);
+	if (err == -ENOTSUPP)
 	err = rt_ip_route_output(&rt, daddr, saddr);
 	if (err)
 		goto out;
@@ -670,7 +704,8 @@ ssize_t rt_udp_sendmsg(struct rtdm_fd *fd, const struct user_msghdr *msg,
 	err = rt_ip_build_xmit(sock, rt_udp_getfrag, &ufh, ulen, &rt,
 			       msg_flags);
 
-	/* Drop the reference obtained in rt_ip_route_output() */
+	/* Drop the reference obtained in either rt_ip_route_output()
+	   or route_multicast() */
 	rtdev_dereference(rt.rtdev);
 out:
 	rtdm_drop_iovec(iov, iov_fast);
@@ -715,7 +750,7 @@ struct rtsocket *rt_udp_dest_socket(struct rtskb *skb)
 			csum_tcpudp_nofold(saddr, daddr, ulen, IPPROTO_UDP, 0);
 
 	/* patch broadcast daddr */
-	if (daddr == rtdev->broadcast_ip)
+	if (daddr == rtdev->broadcast_ip || rtnet_in_multicast(ntohl(daddr)))
 		daddr = rtdev->local_ip;
 
 	/* find the destination socket */
diff --git a/kernel/drivers/net/stack/rtdev.c b/kernel/drivers/net/stack/rtdev.c
index 13e979088..a78d5241f 100644
--- a/kernel/drivers/net/stack/rtdev.c
+++ b/kernel/drivers/net/stack/rtdev.c
@@ -913,6 +913,208 @@ int rtdev_xmit_proxy(struct rtskb *rtskb)
 }
 #endif /* CONFIG_XENO_DRIVERS_NET_ADDON_PROXY */
 
+#ifdef CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP
+
+/* Find rtnet device by its local_ip adress */
+static inline struct rtnet_device * __rt_ip_dev_find(u32 addr){
+    int i;
+    struct rtnet_device *rtdev;
+
+    for (i = 0; i < MAX_RT_DEVICES; i++) {
+        rtdev = rtnet_devices[i];
+        if ((rtdev != NULL) && (rtdev->local_ip == addr)) {
+            return rtdev;
+        }
+    }
+    return NULL;
+}
+
+/***
+ *  rtdev_get_by_hwaddr - find and lock a rtnetdevice by its local ip adress
+ *  @addr:         Local  IP Adress
+  */
+struct rtnet_device *rt_ip_dev_find(u32 addr)
+{
+    struct rtnet_device * rtdev;
+    unsigned long flags;
+
+
+    rtdm_lock_get_irqsave(&rtnet_devices_rt_lock, flags);
+    rtdev = __rt_ip_dev_find(addr);
+    if (rtdev != NULL)
+        atomic_inc(&rtdev->refcount);
+    rtdm_lock_put_irqrestore(&rtnet_devices_rt_lock, flags);
+
+    return rtdev;
+}
+EXPORT_SYMBOL(rt_ip_dev_find);
+
+/*
+ *  Update the multicast list into the physical NIC controller.
+ */
+static void __rt_dev_mc_upload(struct rtnet_device *dev)
+{
+    /* Don't do anything till we up the interface
+     * [dev_open will call this function so the list will
+     * stay sane]
+     */
+
+    if (!(dev->flags&IFF_UP))
+        return;
+
+    /*
+     *  Devices with no set multicast or which have been
+     *  detached don't get set.
+     */
+
+    if (dev->set_multicast_list == NULL){
+        printk(KERN_INFO "RTnet: %s does not support multicast\n",
+	       dev->name);
+        return;
+    }
+    dev->set_multicast_list(dev);
+}
+
+void rt_dev_mc_upload(struct rtnet_device *dev)
+{
+    unsigned long flags;
+    rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+    __rt_dev_mc_upload(dev);
+    rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+}
+
+/*
+ *  Delete a device level multicast
+ */
+
+int rt_dev_mc_delete(struct rtnet_device *dev, void *addr, int alen, int glbl)
+{
+    int err = 0;
+    struct rtdev_mc_list *dmi, **dmip;
+    unsigned long flags;
+
+    rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+
+    for (dmip = &dev->mc_list; (dmi = *dmip) != NULL; dmip = &dmi->next) {
+        /*
+         *  Find the entry we want to delete. The device could
+         *  have variable length entries so check these too.
+         */
+        if (memcmp(dmi->dmi_addr, addr, dmi->dmi_addrlen) == 0 &&
+            alen == dmi->dmi_addrlen) {
+            if (glbl) {
+                int old_glbl = dmi->dmi_gusers;
+                dmi->dmi_gusers = 0;
+                if (old_glbl == 0)
+                    break;
+            }
+            if (--dmi->dmi_users)
+                goto done;
+
+            /*
+             *  Last user. So delete the entry.
+             */
+            *dmip = dmi->next;
+            dev->mc_count--;
+
+            /*
+             *  We have altered the list, so the card
+             *  loaded filter is now wrong. Fix it
+             */
+            __rt_dev_mc_upload(dev);
+
+            rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+
+            rtdm_free(dmi);
+            return 0;
+        }
+    }
+    err = -ENOENT;
+done:
+    rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+    return err;
+}
+EXPORT_SYMBOL(rt_dev_mc_delete);
+
+/*
+ *  Add a device level multicast
+ */
+
+
+int rt_dev_mc_add(struct rtnet_device *dev, void *addr, int alen, int glbl)
+{
+    int err = 0;
+    struct rtdev_mc_list *dmi, *dmi1;
+    unsigned long flags;
+
+    dmi1 = rtdm_malloc(sizeof(*dmi1));
+
+    rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+    for (dmi = dev->mc_list; dmi != NULL; dmi = dmi->next) {
+        if (memcmp(dmi->dmi_addr, addr, dmi->dmi_addrlen) == 0 &&
+            dmi->dmi_addrlen == alen) {
+            if (glbl) {
+                int old_glbl = dmi->dmi_gusers;
+                dmi->dmi_gusers = 1;
+                if (old_glbl)
+                    goto done;
+            }
+            dmi->dmi_users++;
+            goto done;
+        }
+    }
+
+    if ((dmi = dmi1) == NULL) {
+        rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+        return -ENOMEM;
+    }
+    memcpy(dmi->dmi_addr, addr, alen);
+    dmi->dmi_addrlen = alen;
+    dmi->next = dev->mc_list;
+    dmi->dmi_users = 1;
+    dmi->dmi_gusers = glbl ? 1 : 0;
+    dev->mc_list = dmi;
+    dev->mc_count++;
+
+    __rt_dev_mc_upload(dev);
+
+    rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+    return 0;
+
+done:
+    rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+    if (dmi1)
+        rtdm_free(dmi1);
+    return err;
+}
+EXPORT_SYMBOL(rt_dev_mc_add);
+
+/*
+ *  Discard multicast list when a device is downed
+ */
+
+void rt_dev_mc_discard(struct rtnet_device *dev)
+{
+    unsigned long flags;
+
+    rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+
+    while (dev->mc_list != NULL) {
+        struct rtdev_mc_list *tmp = dev->mc_list;
+        dev->mc_list = tmp->next;
+        if (tmp->dmi_users > tmp->dmi_gusers)
+            printk("dev_mc_discard: multicast leakage! dmi_users=%d\n", tmp->dmi_users);
+        rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+        rtdm_free(tmp);
+        rtdm_lock_get_irqsave(&dev->rtdev_lock, flags);
+    }
+    dev->mc_count = 0;
+
+    rtdm_lock_put_irqrestore(&dev->rtdev_lock, flags);
+}
+
+#endif	/* CONFIG_XENO_DRIVERS_NET_RTIPV4_IGMP */
+
 unsigned int rt_hard_mtu(struct rtnet_device *rtdev, unsigned int priority)
 {
 	return rtdev->mtu;
-- 
2.17.1




More information about the Xenomai mailing list