devinet.c

一个基于linux的TCP/IP协议栈的实现

/* devinet.c
 * [e-mail]
 * 2006-10-15
 */

#include "devinet.h"
#include "log.h"
#include "af_inet.h"
#include "neighbour.h"
#include "igmp.h"

#include <linux/inetdevice.h>
#include <linux/rtnetlink.h>
#include <linux/in.h>

extern struct neigh_table myarp_tbl;

static struct in_ifaddr *myinet_alloc_ifa(void)
{
	struct in_ifaddr *ifa = kmalloc( sizeof(*ifa), GFP_KERNEL );
	if( ifa ){
		memset( ifa, 0, sizeof(*ifa) );
		INIT_RCU_HEAD(&ifa->rcu_head);
	}
	return ifa;
}

void myin_dev_finish_destroy(struct in_device *idev)
{
	struct net_device *dev = idev->dev;

	BUG_TRAP(!idev->ifa_list);
	BUG_TRAP(!idev->mc_list);
#ifdef NET_REFCNT_DEBUG
	printk(KERN_DEBUG "in_dev_finish_destroy: %p=%s\n",
	       idev, dev ? dev->name : "NIL");
#endif
	dev_put(dev);
	if (!idev->dead)
		printk("Freeing alive in_device %p\n", idev);
	else {
		kfree(idev);
	}
}

static void myinet_rcu_free_ifa( struct rcu_head *head )
{
	struct in_ifaddr *ifa = container_of(head, struct in_ifaddr, rcu_head);
	if (ifa->ifa_dev)
		in_dev_put(ifa->ifa_dev);
	kfree(ifa);
}

static inline void myinet_free_ifa( struct in_ifaddr *ifa )
{
	call_rcu( &ifa->rcu_head, myinet_rcu_free_ifa );
}

static int myinet_insert_ifa(struct in_ifaddr *ifa)
{
	struct in_device *in_dev = ifa->ifa_dev;
	struct in_ifaddr *ifa1, **ifap, **last_primary;

	ASSERT_RTNL();

	if( !ifa->ifa_local ){
		myinet_free_ifa( ifa );
		return 0;
	}

	ifa->ifa_flags &= ~IFA_F_SECONDARY;
	last_primary = &in_dev->ifa_list;

	for( ifap = &in_dev->ifa_list; (ifa1 = *ifap) != NULL; ifap = &ifa1->ifa_next ){
		if( !(ifa1->ifa_flags & IFA_F_SECONDARY) && ifa->ifa_scope <= ifa1->ifa_scope )
			last_primary = &ifa1->ifa_next;
		if( ifa1->ifa_mask == ifa->ifa_mask && inet_ifa_match(ifa1->ifa_address, ifa) ){
			if( ifa1->ifa_local == ifa->ifa_local ){
				myinet_free_ifa(ifa);
				return -EEXIST;
			}
			if( ifa1->ifa_scope != ifa->ifa_scope ){
				myinet_free_ifa(ifa);
				return -EINVAL;
			}
			ifa->ifa_flags |= IFA_F_SECONDARY;
		}
	}
	if( !(ifa->ifa_flags & IFA_F_SECONDARY) )
		return 0;
	ifa->ifa_next = *ifap;
	*ifap = ifa;

	return 0;
}

static int myinet_del_ifa(struct in_device *in_dev, struct in_ifaddr **ifap,
				int destroy)
{
	struct in_ifaddr *ifa1 = *ifap;

	ASSERT_RTNL();

	if( !(ifa1->ifa_flags & IFA_F_SECONDARY) )
		return -1;
	*ifap = ifa1->ifa_next;
	return 0;
}

void myarp_ifdown( struct net_device *dev )
{
	myneigh_ifdown( &myarp_tbl, dev );
}

static __inline__ int myinet_abc_len(u32 addr)
{
	int rc = -1;

  	if( ZERONET(addr) )
  		rc = 0;
	else{
		addr = ntohl(addr);
		if (IN_CLASSA(addr))
			rc = 8;
		else if (IN_CLASSB(addr))
			rc = 16;
		else if (IN_CLASSC(addr))
			rc = 24;
	}
  	return rc;
}

static int myinet_set_ifa(struct net_device *dev, struct in_ifaddr *ifa)
{
	struct in_device *in_dev = __in_dev_get_rtnl(dev);
	ASSERT_RTNL();
	if (!in_dev) {
		return -ENOBUFS;
	}
	if( ifa->ifa_dev != in_dev ){
		BUG_TRAP( !ifa->ifa_dev );
		in_dev_hold(in_dev);
		ifa->ifa_dev = in_dev;
	}
	if( LOOPBACK(ifa->ifa_local) )
		ifa->ifa_scope = RT_SCOPE_HOST;
	return myinet_insert_ifa(ifa);
}

int mydevinet_ioctl(unsigned int cmd, void __user *arg)
{
	struct ifreq ifr;
	struct sockaddr_in sin_orig;
	struct sockaddr_in *sin = (struct sockaddr_in *)&ifr.ifr_addr;
	char *colon;
	struct in_device *in_dev;
	struct in_ifaddr **ifap = NULL;
	struct in_ifaddr *ifa = NULL;
	struct net_device *dev;
	int ret = -EFAULT;
	int tryaddrmatch = 0;

	if( copy_from_user(&ifr, arg, sizeof(struct ifreq)) )
		goto out;
	ifr.ifr_name[IFNAMSIZ - 1] = 0;
	memcpy(&sin_orig, sin, sizeof(*sin));

	colon = strchr(ifr.ifr_name, ':');
	if( colon )
		*colon = 0;
#ifdef CONFIG_KMOD
	dev_load(ifr.ifr_name);
#endif

	switch(cmd) {
		case SIOCGIFADDR:
		case SIOCGIFBRDADDR:
		case SIOCGIFDSTADDR:
		case SIOCGIFNETMASK:
				tryaddrmatch = (sin_orig.sin_family == MY_AF_INET);
				memset( sin, 0, sizeof(*sin) );
				sin->sin_family = MY_AF_INET;
				break;
		case SIOCSIFFLAGS:
				ret = -EACCES;
				if( !capable(CAP_NET_ADMIN) )
					goto out;
				break;
		case SIOCSIFADDR:
		case SIOCSIFBRDADDR:
		case SIOCSIFDSTADDR:
		case SIOCSIFNETMASK:
				ret = -EACCES;
				if (!capable(CAP_NET_ADMIN))
						goto out;
				ret = -EINVAL;
				if (sin->sin_family != MY_AF_INET)
						goto out;
				break;
		default:
				ret = -EINVAL;
				goto out;
	}

	rtnl_lock();
	ret = -ENODEV;
	if( (dev = __dev_get_by_name(ifr.ifr_name)) == NULL )
		goto done;
	if( colon )
		*colon = ':';

	if( (in_dev = __in_dev_get_rtnl(dev)) != NULL ){
		if( tryaddrmatch ){
			for( ifap = &in_dev->ifa_list; (ifa = *ifap) != NULL; ifap = &ifa->ifa_next ){
				if( !strcmp(ifr.ifr_name, ifa->ifa_label) &&
				    sin_orig.sin_addr.s_addr == ifa->ifa_address) {
					break;
				}
			}
		}
		if( !ifa ){
			for( ifap = &in_dev->ifa_list; (ifa = *ifap) != NULL; ifap = &ifa->ifa_next )
				if( !strcmp(ifr.ifr_name, ifa->ifa_label) )
					break;
		}
	}

	ret = -EADDRNOTAVAIL;
	if( !ifa && cmd != SIOCSIFADDR && cmd != SIOCSIFFLAGS )
		goto done;

	switch(cmd) {
		case SIOCGIFADDR:
			sin->sin_addr.s_addr = ifa->ifa_local;
			goto rarok;
		case SIOCGIFBRDADDR:
			sin->sin_addr.s_addr = ifa->ifa_broadcast;
			goto rarok;
		case SIOCGIFDSTADDR:
			sin->sin_addr.s_addr = ifa->ifa_address;
			goto rarok;
		case SIOCGIFNETMASK:
			sin->sin_addr.s_addr = ifa->ifa_mask;
			goto rarok;
		case SIOCSIFBRDADDR:
			ret = 0;
			if( ifa->ifa_broadcast != sin->sin_addr.s_addr ){
				if( myinet_del_ifa(in_dev, ifap, 0) >= 0 ){
					ifa->ifa_broadcast = sin->sin_addr.s_addr;
					myinet_insert_ifa(ifa);
				}
			}
			break;
		case SIOCSIFADDR:
			ret = -EINVAL;
			if( myinet_abc_len(sin->sin_addr.s_addr) < 0)
				break;
			if( !ifa ){
				ret = -ENOBUFS;
				if( (ifa = myinet_alloc_ifa()) == NULL )
					break;
				if( colon )
					memcpy(ifa->ifa_label, ifr.ifr_name, IFNAMSIZ);
				else
					memcpy(ifa->ifa_label, dev->name, IFNAMSIZ);
			}else{
				ret = 0;
				if (ifa->ifa_local == sin->sin_addr.s_addr)
					break;
				if( myinet_del_ifa(in_dev, ifap, 0) >= 0 ){
					ifa->ifa_broadcast = 0;
					ifa->ifa_anycast = 0;
				}else
					return -EINVAL;
			}
			ifa->ifa_address = ifa->ifa_local = sin->sin_addr.s_addr;
			if( !(dev->flags & IFF_POINTOPOINT) ){
				ifa->ifa_prefixlen = myinet_abc_len(ifa->ifa_address);
				ifa->ifa_mask = inet_make_mask(ifa->ifa_prefixlen);
				if( (dev->flags & IFF_BROADCAST) && ifa->ifa_prefixlen < 31 )
					ifa->ifa_broadcast = ifa->ifa_address | ~ifa->ifa_mask;
			}else{
				ifa->ifa_prefixlen = 32;
				ifa->ifa_mask = inet_make_mask(32);
			}
			ret = myinet_set_ifa(dev, ifa);
			break;
		case SIOCSIFFLAGS:
			return -EINVAL;
		case SIOCSIFNETMASK:
			ret = -EINVAL;
			if( bad_mask(sin->sin_addr.s_addr, 0) )
				break;
			ret = 0;
			if (ifa->ifa_mask != sin->sin_addr.s_addr) {
				u32 old_mask = ifa->ifa_mask;
				if( myinet_del_ifa(in_dev, ifap, 0) < 0 )
					return -EINVAL;
				ifa->ifa_mask = sin->sin_addr.s_addr;
				ifa->ifa_prefixlen = inet_mask_len(ifa->ifa_mask);
				if( (dev->flags & IFF_BROADCAST) && (ifa->ifa_prefixlen < 31) &&
								(ifa->ifa_broadcast == (ifa->ifa_local|~old_mask))) {
					ifa->ifa_broadcast = (ifa->ifa_local | ~sin->sin_addr.s_addr);
				}
				myinet_insert_ifa(ifa);
			}
			break;
		case SIOCSIFDSTADDR:
			ret = 0;
			if (ifa->ifa_address == sin->sin_addr.s_addr)
				break;
			ret = -EINVAL;
			if( myinet_abc_len(sin->sin_addr.s_addr) < 0 )
				break;
			ret = 0;
			if( myinet_del_ifa(in_dev, ifap, 0) < 0 )
				return -EINVAL;
			ifa->ifa_address = sin->sin_addr.s_addr;
			myinet_insert_ifa(ifa);
			break;
	}
done:
	rtnl_unlock();
out:
	return ret;
rarok:
	rtnl_unlock();
	ret = copy_to_user(arg, &ifr, sizeof(struct ifreq)) ? -EFAULT : 0;
	goto out;
}

static int myinet_gifconf(struct net_device *dev, char __user *buf, int len)
{
	struct in_device *in_dev = __in_dev_get_rtnl(dev);
	struct in_ifaddr *ifa;
	struct ifreq ifr;
	int done = 0;

	if( !in_dev || (ifa = in_dev->ifa_list) == NULL )
		goto out;

	for (; ifa; ifa = ifa->ifa_next) {
		if( !buf ){
			done += sizeof(ifr);
			continue;
		}
		if( len < (int) sizeof(ifr) )
			break;
		memset(&ifr, 0, sizeof(struct ifreq));
		if( ifa->ifa_label )
			strcpy(ifr.ifr_name, ifa->ifa_label);
		else
			strcpy(ifr.ifr_name, dev->name);

		(*(struct sockaddr_in *)&ifr.ifr_addr).sin_family = MY_AF_INET;
		(*(struct sockaddr_in *)&ifr.ifr_addr).sin_addr.s_addr = ifa->ifa_local;

		if (copy_to_user(buf, &ifr, sizeof(struct ifreq))) {
			done = -EFAULT;
			break;
		}
		buf  += sizeof(struct ifreq);
		len  -= sizeof(struct ifreq);
		done += sizeof(struct ifreq);
	}
out:
	return done;
}

void __init mydevinet_init(void)
{
	myregister_gifconf( MY_PF_INET, myinet_gifconf );
}

void __exit mydevinet_exit(void)
{
	myunregister_gifconf( MY_PF_INET );
}

struct in_ifaddr *myinet_ifa_byprefix(struct in_device *in_dev, u32 prefix,
				u32 mask)
{
	ASSERT_RTNL();

	for_primary_ifa(in_dev) {
		if (ifa->ifa_mask == mask && inet_ifa_match(prefix, ifa))
			return ifa;
	} endfor_ifa(in_dev);
	return NULL;
}

u32 myinet_select_addr(const struct net_device *dev, u32 dst, int scope)
{
	u32 addr = 0;
	struct in_device *in_dev;

	rcu_read_lock();
	in_dev = __in_dev_get_rcu(dev);
	if( !in_dev )
		goto no_in_dev;

	for_primary_ifa(in_dev) {
		if( ifa->ifa_scope > scope )
			continue;
		if( !dst || inet_ifa_match(dst, ifa) ){
			addr = ifa->ifa_local;
			break;
		}
		if (!addr)
			addr = ifa->ifa_local;
	} endfor_ifa(in_dev);
no_in_dev:
	rcu_read_unlock();
	if( addr )
		goto out;

	read_lock(&dev_base_lock);
	rcu_read_lock();

	for( dev = dev_base; dev; dev = dev->next ){
		if ((in_dev = __in_dev_get_rcu(dev)) == NULL)
			continue;
		for_primary_ifa( in_dev ){
			if( ifa->ifa_scope != RT_SCOPE_LINK && ifa->ifa_scope <= scope ){
				addr = ifa->ifa_local;
				goto out_unlock_both;
			}
		}endfor_ifa(in_dev);
	}
out_unlock_both:
	read_unlock( &dev_base_lock );
	rcu_read_unlock();
out:
	return addr;
}

EXPORT_SYMBOL_GPL( mydevinet_ioctl );
EXPORT_SYMBOL_GPL( myinet_ifa_byprefix );
EXPORT_SYMBOL_GPL( myinet_select_addr );