gsn_vlan.c

Linux下通过设计虚拟网卡来实现VLAN功能

#include <linux/module.h>
#include <asm/uaccess.h>
#include <asm/system.h>
#include <linux/bitops.h>
#include <linux/capability.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/mm.h>
#include <linux/string.h>
#include <linux/socket.h>
#include <linux/sockios.h>
#include <linux/errno.h>
#include <linux/in.h>
#include <linux/inet.h>
#include <linux/inetdevice.h>
#include <linux/netdevice.h>
#include <linux/if_addr.h>
#include <linux/if_arp.h>
#include <linux/skbuff.h>
#include <linux/netlink.h>
#include <linux/init.h>
#include <linux/list.h>
#include <linux/if_addr.h>
#include <linux/if_ether.h>
 
#include <net/ip.h>
#include <net/protocol.h>
#include <net/tcp.h>
#include <net/sock.h>
#include <net/icmp.h>
#include <net/arp.h>
#include <net/ip_fib.h>

#include "gsn_vlan.h"

MODULE_LICENSE("Dual BSD/GPL");

/* ============================== */
#define VLAN_GRP_HASH_SHIFT   5
#define VLAN_GRP_HASH_SIZE (1 << VLAN_GRP_HASH_SHIFT)
#define VLAN_GRP_HASH_MASK (VLAN_GRP_HASH_SIZE - 1)

static int vlan_device_event(struct notifier_block *, unsigned long, void *);
static int unregister_vlan_dev(struct net_device *, unsigned short);


/* used for vlan visual device */
static struct notifier_block vlan_notifier_block = {
	.notifier_call = vlan_device_event,
};

static struct packet_type vlan_packet_type = {
	.type = __constant_htons(ETH_P_8021Q),
	.func = vlan_skb_recv, 
};


/* Our listing of VLAN group(s) */
static struct hlist_head vlan_group_hash[VLAN_GRP_HASH_SIZE];
static struct lock_class_key vlan_netdev_xmit_lock_key;
#define vlan_grp_hashfn(IDX)  ((((IDX) >> VLAN_GRP_HASH_SHIFT) ^ (IDX)) & VLAN_GRP_HASH_MASK)


/* ======== xxxx add the local route table ======= */
extern void fib_add_ifaddr(struct in_ifaddr *);


/* ------ function ---------------- */

static inline u_int32_t make_mask(int mask_len)
{
	if (mask_len) {
		return htonl(~((1<<(32 - mask_len)) - 1));
	}

	return 0;
}

int vlan_do_skb(struct sk_buff *skb, struct net_device *dev,
		struct packet_type *ptype, struct net_device *orig_dev)
{
	struct vlan_hdr *vhdr = (struct vlan_hdr *)(skb->data);
	unsigned short vid;
	struct net_device_stats *stats;
	unsigned short vlan_TCI;
	__be16 proto;

	vlan_TCI = ntohs(vhdr->h_vlan_TCI);

	vid = (vlan_TCI & VLAN_VID_MASK);

	rcu_read_lock();
	skb->dev = __find_vlan_dev(dev, vid);
	/* from now, the skb->dev is the vlan dev*/

	if (!skb->dev) {
		rcu_read_unlock();
		/* XXX if we not register the VLAN, Should I deal with it ?*/
		kfree_skb(skb);
		return -1;
	}

	skb->dev->last_rx = jiffies;
	
	stats = vlan_dev_get_stats(skb->dev);
	stats->rx_packets++;
	stats->rx_bytes += skb->len;

	skb_pull_rcsum(skb, VLAN_HLEN);

	if (dev != VLAN_DEV_INFO(skb->dev)->real_dev) {
		rcu_read_unlock();
		printk("%s: dropping skb: %p because came in on wrong device, dev: %s  real_dev: %s, skb_dev: %s\n",
				__FUNCTION__, skb, dev->name,
				VLAN_DEV_INFO(skb->dev)->real_dev->name,
				skb->dev->name);
		kfree_skb(skb);
		stats->rx_errors++;
		return -1;
	}

	skb->priority = vlan_get_ingress_priority(skb->dev, ntohs(vhdr->h_vlan_TCI));

	switch (skb->pkt_type) {
		case PACKET_BROADCAST:
			break;

		case PACKET_MULTICAST:
			stats->multicast++;
			break;

		case PACKET_OTHERHOST:
			if (!compare_ether_addr(eth_hdr(skb)->h_dest, skb->dev->dev_addr)) {
				skb->pkt_type = PACKET_HOST;
			}
			break;
	}

	proto = vhdr->h_vlan_encapsulated_proto;
	skb->protocol = proto;
	
	rcu_read_unlock();
	return 0;
}


static inline struct sk_buff *vlan_check_reorder_header(struct sk_buff *skb)
{
	if (VLAN_DEV_INFO(skb->dev)->flags & 1) {
		if (skb_shared(skb) || skb_cloned(skb)) {
			struct sk_buff *nskb = skb_copy(skb, GFP_ATOMIC);
			kfree_skb(skb);
			skb = nskb;
		}
		if (skb) {
			/* the memmove is so good, when dest > src, the cope from last to first */
			memmove(skb->data - ETH_HLEN,
					skb->data - VLAN_ETH_HLEN, 12);
			skb->mac.raw += VLAN_HLEN;
		}
	}

	return skb;
}


int vlan_skb_recv(struct sk_buff *skb, struct net_device *dev,
		struct packet_type *ptype, struct net_device *orig_dev)
{
	if (vlan_do_skb(skb, dev, ptype, orig_dev) == -1)
		return -1;

	if (skb->protocol == __constant_htons(ETH_P_8021Q)) {
		printk("vlan_do_skb BUG!\n");
		kfree(skb);
		return -1;
	}

	if (ntohs(skb->protocol) >= 1536) {
		skb = vlan_check_reorder_header(skb);
		if (skb) {
			netif_rx(skb);
		} else {
			/*
			stats->rx_errors++;
			*/
			printk("%s:skb error\n", __FUNCTION__);
		}

		return 0;
	} 

	/* should alnayse more ??? */
	printk("%s: the skb protocol no valid(should I deal with it ?), drop the skb\n", __FUNCTION__);
	kfree(skb);
	return -1;
}


static void route_add(void * parm, const char *eth_IF_name)
{
	struct in_ifaddr * ifa;
	struct net_device *dev = dev_get_by_name(eth_IF_name);
	ifa = kmalloc(sizeof(struct in_ifaddr), GFP_ATOMIC);
	if (!ifa) {
		printk("error\n");
		return ;
	}

	ifa->ifa_dev = in_dev_get(dev);
	ifa->ifa_mask = make_mask(24);
	ifa->ifa_local = 0;
	ifa->ifa_address = 255240384;
	ifa->ifa_prefixlen = 24;

	fib_add_ifaddr(ifa);

	in_dev_put(ifa->ifa_dev);
	dev_put(dev);
	
	kfree(ifa);
	return ;
}

static struct vlan_group * __vlan_find_group(int real_dev_ifindex)
{
	struct vlan_group *grp;
	struct hlist_node *n;
	int hash = vlan_grp_hashfn(real_dev_ifindex);

	hlist_for_each_entry_rcu(grp, n, &vlan_group_hash[hash], hlist) {
		if (grp->real_dev_ifindex == real_dev_ifindex)
			return grp;
	}

	return NULL;
}


struct net_device * __find_vlan_dev(struct net_device * real_dev,
		unsigned short VID)
{
	struct vlan_group * grp = __vlan_find_group(real_dev->ifindex);

	if (grp)
		return vlan_group_get_device(grp, VID);

	return NULL;
}

static void vlan_group_free(struct vlan_group *grp)
{
	int i;

	for (i=0; i < VLAN_GROUP_ARRAY_SPLIT_PARTS; i++)
		kfree(grp->vlan_devices_arrays[i]);
	kfree(grp);
}  

static void vlan_rcu_free(struct rcu_head *rcu)
{  
	vlan_group_free(container_of(rcu, struct vlan_group, rcu));
}
   

static void vlan_setup(struct net_device *new_dev)
{
	SET_MODULE_OWNER(new_dev);
	new_dev->get_stats = vlan_dev_get_stats;
	new_dev->priv_flags |= IFF_802_1Q_VLAN;
	new_dev->tx_queue_len = 0;

	new_dev->open = vlan_dev_open;
	new_dev->stop = vlan_dev_stop;
	new_dev->do_ioctl = vlan_dev_ioctl;
	new_dev->destructor = free_netdev;

	/* this should be called but I do it nothing */
	new_dev->change_mtu = vlan_dev_change_mtu;
	new_dev->set_mac_address = vlan_dev_set_mac_address;
	new_dev->set_multicast_list = vlan_dev_set_multicast_list;

}


static void vlan_transfer_operstate(const struct net_device *dev, struct net_device *vlandev)
{
	if (dev->operstate == IF_OPER_DORMANT)
		netif_dormant_on(vlandev);
	else
		netif_dormant_off(vlandev);

	if (netif_carrier_ok(dev)) {
		if (!netif_carrier_ok(vlandev))
			netif_carrier_on(vlandev);
	} else {
		if (netif_carrier_ok(vlandev))
			netif_carrier_off(vlandev);
	}
}


static int vlan_device_event(struct notifier_block *unused, unsigned long event, void *ptr)
{
	struct net_device *dev = ptr;
	struct vlan_group *grp = __vlan_find_group(dev->ifindex);
	int i, flags;
	struct net_device *vlandev;

	if (!grp)
		goto out;

	switch(event) {
		case NETDEV_CHANGE:
			for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) {
				vlandev = vlan_group_get_device(grp, i);
				if (!vlandev)
					continue;

				vlan_transfer_operstate(dev, vlandev);
			}
			break;

		case NETDEV_DOWN:
			for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) {
				vlandev = vlan_group_get_device(grp, i);
				if (!vlandev)
					continue;

				flags = vlandev->flags;
				if (!(flags & IFF_UP))
					continue;

				dev_change_flags(vlandev, flags & ~IFF_UP);
			}
			break;

		case NETDEV_UP:
			for (i = 0; i < VLAN_GROUP_ARRAY_PART_LEN; i++) {
				vlandev = vlan_group_get_device(grp, i);
				if (!vlandev)
					continue;

				flags = vlandev->flags;
				if (flags & IFF_UP)
					continue;

				dev_change_flags(vlandev, flags | IFF_UP);
			}
			break;

		case NETDEV_UNREGISTER:
			for (i = 0; i < VLAN_GROUP_ARRAY_LEN; i++) {
				int ret;
				vlandev = vlan_group_get_device(grp, i);
				if (!vlandev)
					continue;

				ret = unregister_vlan_dev(dev, VLAN_DEV_INFO(vlandev)->vlan_id);
				unregister_netdevice(vlandev);

				if (ret == 1)
					break;
			}
			break;
	};

out:
	return NOTIFY_DONE;
}


static struct 
net_device * register_vlan_device(const char *eth_name, unsigned short VLAN_ID)
{
	struct vlan_group *grp;
	struct net_device *new_dev;
   struct net_device *real_dev;
	char name[IFNAMSIZ];
	int i;

	if (VLAN_ID >= VLAN_VID_MASK)
		goto out_ret_null;

	real_dev = dev_get_by_name(eth_name);
	if (real_dev == NULL)
		goto out_ret_null;

	rtnl_lock();

	if (!(real_dev->flags & IFF_UP))
		goto out_unlock;

	if (__find_vlan_dev(real_dev, VLAN_ID) != NULL) {
		printk("%s: ALREADY had VLAN%d registered\n", __FUNCTION__, VLAN_ID); 
		goto out_unlock;
	}

	snprintf(name, IFNAMSIZ, "%s.vlan%i", real_dev->name, VLAN_ID);

	new_dev = alloc_netdev(sizeof(struct vlan_dev_info), name, vlan_setup);

	if (new_dev == NULL)
		goto out_unlock;

	new_dev->flags = real_dev->flags;
	new_dev->flags &= ~IFF_UP;

	new_dev->state = (real_dev->state & ((1<<__LINK_STATE_NOCARRIER) |
				(1<<__LINK_STATE_DORMANT))) |
		(1<<__LINK_STATE_PRESENT);
	
	new_dev->mtu = real_dev->mtu;
	new_dev->type = real_dev->type;
	new_dev->hard_header_len = real_dev->hard_header_len + VLAN_HLEN; /* XXX */

	memcpy(new_dev->broadcast, real_dev->broadcast, real_dev->addr_len);
	memcpy(new_dev->dev_addr, real_dev->dev_addr, real_dev->addr_len);
	new_dev->addr_len = real_dev->addr_len;

	/* now We not consider the hw accel, just try it */
	new_dev->hard_header = vlan_dev_hard_header;
	new_dev->hard_start_xmit = vlan_dev_hard_start_xmit;
	new_dev->rebuild_header = vlan_dev_rebuild_header;

	new_dev->hard_header_parse = real_dev->hard_header_parse;

	VLAN_DEV_INFO(new_dev)->vlan_id = VLAN_ID;
	VLAN_DEV_INFO(new_dev)->real_dev = real_dev;
	VLAN_DEV_INFO(new_dev)->dent = NULL;
	VLAN_DEV_INFO(new_dev)->flags = 1;

	if (register_netdevice(new_dev))
		goto out_free_newdev;
	

	lockdep_set_class(&new_dev->_xmit_lock, &vlan_netdev_xmit_lock_key);

	new_dev->iflink = real_dev->ifindex;
	vlan_transfer_operstate(real_dev, new_dev);
	linkwatch_fire_event(new_dev); /* _MUST_ call rfc2863_policy() */

	grp = __vlan_find_group(real_dev->ifindex);

	if (!grp) {
		grp = kzalloc(sizeof(struct vlan_group), GFP_KERNEL);
		if (!grp)
			goto out_free_unregister;

		for (i = 0; i < VLAN_GROUP_ARRAY_SPLIT_PARTS; i++) {
			grp->vlan_devices_arrays[i] = kzalloc(
					sizeof(struct net_device *) * VLAN_GROUP_ARRAY_PART_LEN,
					GFP_KERNEL);

			if (!grp->vlan_devices_arrays[i])
				goto out_free_arrays;
		}

		grp->real_dev_ifindex = real_dev->ifindex;

		hlist_add_head_rcu(&grp->hlist,
				&vlan_group_hash[vlan_grp_hashfn(real_dev->ifindex)]);
	}

	vlan_group_set_device(grp, VLAN_ID, new_dev);

	rtnl_unlock();

	printk("Allocated new device successfully, returning.\n");

	return new_dev;

out_free_arrays:
	vlan_group_free(grp);

out_free_unregister:
	unregister_netdev(new_dev);
	goto out_unlock;

out_free_newdev:
	free_netdev(new_dev);

out_unlock:
	rtnl_unlock();

out_ret_null:
	return NULL;
}


static int unregister_vlan_device(const char *vlan_IF_name)
{
	struct net_device *dev = NULL;
	int ret;
	dev = dev_get_by_name(vlan_IF_name);
	ret = -EINVAL;

	if (dev) {
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			rtnl_lock();

			ret = unregister_vlan_dev(VLAN_DEV_INFO(dev)->real_dev,
					VLAN_DEV_INFO(dev)->vlan_id);

			dev_put(dev);
			unregister_netdevice(dev);

			rtnl_unlock();
			if (ret == 1)
				ret = 0;
		} else {
			printk("%s: ERROR \n", __FUNCTION__);
			dev_put(dev);
			ret = -EPERM;
		}
	} else {
		 printk("%s: WARNING: Could not find dev.\n", __FUNCTION__);
		 ret = -EINVAL;
	}

	return ret ;
}


static int unregister_vlan_dev(struct net_device *real_dev,
		unsigned short vlan_id)
{
	struct net_device *dev = NULL;
	int real_dev_ifindex = real_dev->ifindex;
	struct vlan_group *grp;
	int i, ret;

	if (vlan_id >= VLAN_VID_MASK)
		return -EINVAL;

	ASSERT_RTNL();

	grp = __vlan_find_group(real_dev_ifindex);
	ret = 0;

	if (grp) {
		dev = vlan_group_get_device(grp, vlan_id);
		if (dev) {
			vlan_group_set_device(grp, vlan_id, NULL);
			synchronize_net();

			dev_put(real_dev);

			for (i = 0; i < VLAN_VID_MASK; i++)
				if (vlan_group_get_device(grp, i))
					break;

			if (i == VLAN_VID_MASK) {
				hlist_del_rcu(&grp->hlist);
				call_rcu(&grp->rcu, vlan_rcu_free);
				grp = NULL;
				ret = 1;
			}
		}
	}

	return ret;
}


static int tmod_init(void)
{
/*	register_vlan_device("eth2", 3); */
	int err;
	dev_add_pack(&vlan_packet_type);
	err = register_netdevice_notifier(&vlan_notifier_block);

	if (err < 0) {
		dev_remove_pack(&vlan_packet_type);
		return err;
	}

	register_vlan_device("eth2", 888);

	return 0;
}


static void vlan_cleanup_devices(void)
{
	struct net_device *dev, *nxt;

	rtnl_lock();
	for (dev = dev_base; dev; dev = nxt) {
		nxt = dev->next;
		if (dev->priv_flags & IFF_802_1Q_VLAN) {
			unregister_vlan_dev(VLAN_DEV_INFO(dev)->real_dev,
					VLAN_DEV_INFO(dev)->vlan_id);

			unregister_netdevice(dev);
		}
	}
	rtnl_unlock();
}


static void tmod_exit(void)
{
	int i;
	unregister_netdevice_notifier(&vlan_notifier_block);
	dev_remove_pack(&vlan_packet_type);
	
	vlan_cleanup_devices();

	for (i = 0; i < VLAN_GRP_HASH_SIZE; i++) {
		BUG_ON(!hlist_empty(&vlan_group_hash[i]));
	}

	synchronize_net();
}

module_init(tmod_init);
module_exit(tmod_exit);