mydevinet.c

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

{ 

}

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;
}

struct neigh_table myarp_tbl;
struct in_device *myinetdev_init(struct net_device *dev)
{
	printk(KERN_INFO "go to myinetdev_init!!\n");
	struct in_device *in_dev;

	ASSERT_RTNL();

	in_dev = kmalloc(sizeof(*in_dev), GFP_KERNEL);
	if (!in_dev)
		goto out;
	memset(in_dev, 0, sizeof(*in_dev));

	INIT_RCU_HEAD(&in_dev->rcu_head);
	memcpy(&in_dev->cnf, &myipv4_devconf_dflt, sizeof(in_dev->cnf));
	in_dev->cnf.sysctl = NULL;
	in_dev->dev = dev;
	if ((in_dev->arp_parms = neigh_parms_alloc(dev, &myarp_tbl)) == NULL)
		goto out_kfree;
	dev_hold(dev);
#ifdef CONFIG_SYSCTL
	neigh_sysctl_register(dev, in_dev->arp_parms, NET_MYIPV4,
					NET_IPV4_NEIGH, "ipv4", NULL, NULL);
#endif
	in_dev_hold(in_dev);
	rcu_assign_pointer(dev->ip_ptr, in_dev);
#ifdef CONFIG_SYSCTL
	mydevinet_sysctl_register(in_dev, &in_dev->cnf);
#endif
	myip_mc_init_dev(in_dev);
	if (dev->flags & IFF_UP)
		myip_mc_up(in_dev);

out:
	return in_dev;
out_kfree:
	kfree(in_dev);
	in_dev = NULL;
	goto out;
}

static int myinetdev_event(struct notifier_block *this, unsigned long event,
			 void *ptr)
{
	struct net_device *dev = ptr;
	struct in_device *in_dev = __in_dev_get_rtnl(dev);
	printk(KERN_INFO "%s %d\n", dev->name, event);
	ASSERT_RTNL();

	if (!in_dev) {
		if (event == NETDEV_REGISTER && dev == &loopback_dev) {
			in_dev = myinetdev_init(dev);
			if (!in_dev)
				panic("devinet: Failed to create loopback\n");
			in_dev->cnf.no_xfrm = 1;
			in_dev->cnf.no_policy = 1;
		}
		goto out;
	}else{
#if 0
		printk(KERN_INFO "%s\n", dev->name );
		struct in_ifaddr *ifptr = in_dev->ifa_list;
		struct ip_mc_list *mcptr = in_dev->mc_list;
		do{
			//printk(KERN_INFO "\tmc addr: %u.%u.%u.%u\n", NIPQUAD(mcptr->multiaddr));
			//printk(KERN_INFO "\tlocal: %u.%u.%u.%u\n", NIPQUAD(ifptr->ifa_local) );
			//printk(KERN_INFO "\taddrs: %u.%u.%u.%u\n", NIPQUAD(ifptr->ifa_address) );
			//printk(KERN_INFO "\tbroadcast: %u.%u.%u.%u\n", NIPQUAD(ifptr->ifa_broadcast) );
			//printk(KERN_INFO "\tmask: %x\n", ifptr->ifa_mask );
			//printk(KERN_INFO "\tprefix len: %d\n", ifptr->ifa_prefixlen );
			//printk(KERN_INFO "\tscope: %d\n", ifptr->ifa_scope );
			//printk(KERN_INFO "\tanycast: %lu\n", ifptr->ifa_anycast );
			//printk(KERN_INFO "\tflag: %d\n", ifptr->ifa_flags);
			//printk(KERN_INFO "\tname: %s\n", ifptr->ifa_label);
		}while( mcptr->next != NULL && (mcptr = mcptr->next) );
#endif		
	}

	switch (event) {
	case NETDEV_REGISTER:
		break;
	case NETDEV_UP:
		if (dev->mtu < 68)
			break;
		myip_mc_up(in_dev);
		break;
	case NETDEV_DOWN:
		myip_mc_down(in_dev);
		break;
	case NETDEV_CHANGEMTU:
		if (dev->mtu >= 68)
			break;
	case NETDEV_UNREGISTER:
		myinetdev_destroy(in_dev);
		break;
	case NETDEV_CHANGENAME:
		myinetdev_changename(dev, in_dev);
#ifdef CONFIG_SYSCTL
		mydevinet_sysctl_unregister(&in_dev->cnf);
		neigh_sysctl_unregister(in_dev->arp_parms);
		neigh_sysctl_register(dev, in_dev->arp_parms, NET_MYIPV4,
						NET_IPV4_NEIGH, "ipv4", NULL, NULL);
		mydevinet_sysctl_register(in_dev, &in_dev->cnf);
#endif
		break;
	}
out:
	return NOTIFY_DONE;
}

static int myinet_fill_ifaddr(struct sk_buff *skb, struct in_ifaddr *ifa,
			    u32 pid, u32 seq, int event, unsigned int flags)
{
	struct ifaddrmsg *ifm;
	struct nlmsghdr  *nlh;
	unsigned char	 *b = skb->tail;

	nlh = NLMSG_NEW(skb, pid, seq, event, sizeof(*ifm), flags);
	ifm = NLMSG_DATA(nlh);
	ifm->ifa_family = MY_AF_INET;
	ifm->ifa_prefixlen = ifa->ifa_prefixlen;
	ifm->ifa_flags = ifa->ifa_flags|IFA_F_PERMANENT;
	ifm->ifa_scope = ifa->ifa_scope;
	ifm->ifa_index = ifa->ifa_dev->dev->ifindex;
	if (ifa->ifa_address)
		RTA_PUT(skb, IFA_ADDRESS, 4, &ifa->ifa_address);
	if (ifa->ifa_local)
		RTA_PUT(skb, IFA_LOCAL, 4, &ifa->ifa_local);
	if (ifa->ifa_broadcast)
		RTA_PUT(skb, IFA_BROADCAST, 4, &ifa->ifa_broadcast);
	if (ifa->ifa_anycast)
		RTA_PUT(skb, IFA_ANYCAST, 4, &ifa->ifa_anycast);
	if (ifa->ifa_label[0])
		RTA_PUT(skb, IFA_LABEL, IFNAMSIZ, &ifa->ifa_label);
	nlh->nlmsg_len = skb->tail - b;
	return skb->len;

nlmsg_failure:
rtattr_failure:
	skb_trim(skb, b - skb->data);
	return -1;
}

static void myrtmsg_ifa(int event, struct in_ifaddr* ifa)
{
	int size = NLMSG_SPACE(sizeof(struct ifaddrmsg) + 128);
	struct sk_buff *skb = alloc_skb(size, GFP_KERNEL);

	if (!skb)
		netlink_set_err(rtnl, 0, RTNLGRP_IPV4_IFADDR, ENOBUFS);
	else if( myinet_fill_ifaddr(skb, ifa, 0, 0, event, 0) < 0 ){
		kfree_skb(skb);
		netlink_set_err(rtnl, 0, RTNLGRP_IPV4_IFADDR, EINVAL);
	}else{
		netlink_broadcast(rtnl, skb, 0, RTNLGRP_IPV4_IFADDR, GFP_KERNEL);
	}
}

static void myinet_del_ifa(struct in_device *in_dev, struct in_ifaddr **ifap,
				int destroy)
{
	struct in_ifaddr *promote = NULL;
	struct in_ifaddr *ifa, *ifa1 = *ifap;
	struct in_ifaddr *last_prim = in_dev->ifa_list;
	struct in_ifaddr *prev_prom = NULL;
	int do_promote = MY_IN_DEV_PROMOTE_SECONDARIES(in_dev);

	ASSERT_RTNL();

	if (!(ifa1->ifa_flags & IFA_F_SECONDARY)) {
		struct in_ifaddr **ifap1 = &ifa1->ifa_next;
		while ((ifa = *ifap1) != NULL) {
			if( !(ifa->ifa_flags & IFA_F_SECONDARY ) && ifa1->ifa_scope <= ifa->ifa_scope)
				last_prim = ifa;
			if (!(ifa->ifa_flags & IFA_F_SECONDARY) ||
							ifa1->ifa_mask != ifa->ifa_mask ||
							!inet_ifa_match(ifa1->ifa_address, ifa)) {
					ifap1 = &ifa->ifa_next;
					prev_prom = ifa;
					continue;
			}

			if (!do_promote) {
				*ifap1 = ifa->ifa_next;
				myrtmsg_ifa(RTM_DELADDR, ifa);
				notifier_call_chain(&myinetaddr_chain, NETDEV_DOWN, ifa);
				myinet_free_ifa(ifa);
			}else{
				promote = ifa;
				break;
			}
		}
	}
	*ifap = ifa1->ifa_next;
	myrtmsg_ifa(RTM_DELADDR, ifa1);
	notifier_call_chain(&myinetaddr_chain, NETDEV_DOWN, ifa1);
	
	if( promote ){
		if( prev_prom ){
			prev_prom->ifa_next = promote->ifa_next;
			promote->ifa_next = last_prim->ifa_next;
			last_prim->ifa_next = promote;
		}
		promote->ifa_flags &= ~IFA_F_SECONDARY;
		myrtmsg_ifa(RTM_NEWADDR, promote);
		notifier_call_chain(&myinetaddr_chain, NETDEV_UP, promote);
		for (ifa = promote->ifa_next; ifa; ifa = ifa->ifa_next) {
			if (ifa1->ifa_mask != ifa->ifa_mask ||
							!inet_ifa_match(ifa1->ifa_address, ifa))
				continue;
			myfib_add_ifaddr(ifa);
		}
	}

	if( destroy ){
		myinet_free_ifa(ifa1);
		if (!in_dev->ifa_list)
			myinetdev_destroy(in_dev);
	}
}

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) {
		in_dev = myinetdev_init(dev);
		if (!in_dev) {
			myinet_free_ifa(ifa);
			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;
	struct in_device *in_dev;
	struct in_ifaddr **ifap = NULL;
	struct in_ifaddr *ifa = NULL;
	struct net_device *dev;
	char *colon;
	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 SIOCSIFFLAGS:
			if (colon) {
				ret = -EADDRNOTAVAIL;
				if (!ifa)
					break;
				ret = 0;
				if (!(ifr.ifr_flags & IFF_UP))
					myinet_del_ifa(in_dev, ifap, 1);
				break;
			}
			ret = dev_change_flags(dev, ifr.ifr_flags);
			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;
				myinet_del_ifa(in_dev, ifap, 0);
				ifa->ifa_broadcast = 0;
				ifa->ifa_anycast = 0;
			}
			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 SIOCSIFBRDADDR:
			ret = 0;
			if (ifa->ifa_broadcast != sin->sin_addr.s_addr) {
					myinet_del_ifa(in_dev, ifap, 0);
					ifa->ifa_broadcast = 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;
			myinet_del_ifa(in_dev, ifap, 0);
			ifa->ifa_address = sin->sin_addr.s_addr;
			myinet_insert_ifa(ifa);
			break;
		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;
				myinet_del_ifa(in_dev, ifap, 0);
				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;
	}
done:
	rtnl_unlock();
out:
	return ret;
rarok:
	rtnl_unlock();
	ret = copy_to_user(arg, &ifr, sizeof(struct ifreq)) ? -EFAULT : 0;
	goto out;
}

static struct notifier_block myip_netdev_notifier = {
	.notifier_call = myinetdev_event,
};

static int myinet_gifconf(struct net_device *dev, char __user *buf, int len)
{
	return 0;
}

static void mydevinet_sysctl_register(struct in_device *in_dev,
										struct ipv4_devconf *p)
{
	int i;
	struct net_device *dev = in_dev ? in_dev->dev : NULL;
	struct mydevinet_sysctl_table *t = kmalloc(sizeof(*t), GFP_KERNEL);
	char *dev_name = NULL;

	if (!t)
		return;
	memcpy(t, &mydevinet_sysctl, sizeof(*t));
	for (i = 0; i < ARRAY_SIZE(t->devinet_vars) - 1; i++) {
		t->devinet_vars[i].data += (char *)p - (char *)&myipv4_devconf;
		t->devinet_vars[i].de = NULL;
	}

	if (dev) {
		dev_name = dev->name; 
		t->devinet_dev[0].ctl_name = dev->ifindex;
	} else {
		dev_name = "default";
		t->devinet_dev[0].ctl_name = NET_PROTO_CONF_DEFAULT;
	}

		dev_name = kstrdup(dev_name, GFP_KERNEL);
	if (!dev_name)
	    goto free;

	t->devinet_dev[0].procname    = dev_name;
	t->devinet_dev[0].child	      = t->devinet_vars;
	t->devinet_dev[0].de	      = NULL;
	t->devinet_conf_dir[0].child  = t->devinet_dev;
	t->devinet_conf_dir[0].de     = NULL;
	t->devinet_proto_dir[0].child = t->devinet_conf_dir;
	t->devinet_proto_dir[0].de    = NULL;
	t->devinet_root_dir[0].child  = t->devinet_proto_dir;
	t->devinet_root_dir[0].de     = NULL;

	t->sysctl_header = register_sysctl_table(t->devinet_root_dir, 0);
	if (!t->sysctl_header)
	    goto free_procname;

	p->sysctl = t;
	return;

	/* error path */
 free_procname:
	kfree(dev_name);
 free:
	kfree(t);
	return;
}

static void mydevinet_sysctl_unregister(struct ipv4_devconf *p)
{
	if (p->sysctl) {
		struct mydevinet_sysctl_table *t = p->sysctl;
		p->sysctl = NULL;
		unregister_sysctl_table(t->sysctl_header);
		kfree(t->devinet_dev[0].procname);
		kfree(t);
	}
}
void __init mydevinet_init(void)
{
	register_gifconf(MY_PF_INET, myinet_gifconf);
	register_netdevice_notifier(&myip_netdev_notifier);
	rtnetlink_links[MY_PF_INET] = myinet_rtnetlink_table;
#ifdef CONFIG_SYSCTL
	mydevinet_sysctl.sysctl_header =
		register_sysctl_table(mydevinet_sysctl.devinet_root_dir, 0);
	mydevinet_sysctl_register(NULL, &myipv4_devconf_dflt);
#endif
}

void __exit mydevinet_exit(void)
{
	rtnetlink_links[MY_PF_INET] = NULL;
	unregister_netdevice_notifier( &myip_netdev_notifier );
	unregister_gifconf( MY_PF_INET );

#ifdef CONFIG_SYSCTL
    mydevinet_sysctl_unregister( &myipv4_devconf_dflt );
    unregister_sysctl_table( mydevinet_sysctl.sysctl_header );
#endif	
}