#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);