gsn_vlan_dev.c

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

#include <linux/module.h>
#include <linux/mm.h>
#include <linux/in.h>
#include <linux/init.h>
#include <asm/uaccess.h> /* for copy_from_user */
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <net/datalink.h>
#include <net/p8022.h>
#include <net/arp.h>
 
#include "gsn_vlan.h"
#include <linux/if_vlan.h>
#include <net/ip.h>

int vlan_dev_rebuild_header(struct sk_buff *skb)
{
	struct net_device *dev = skb->dev;
	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)(skb->data);
	printk("%s:why I am called ???\n", __FUNCTION__);
	dump_stack();

	switch (veth->h_vlan_encapsulated_proto) {
#ifdef CONFIG_INET
		case __constant_htons(ETH_P_IP):

			/* TODO:  Confirm this will work with VLAN headers... */
			return arp_find(veth->h_dest, skb);
#endif
		default:
			printk("%s: unable to resolve type %X addresses.\n",
					dev->name, ntohs(veth->h_vlan_encapsulated_proto));

			memcpy(veth->h_source, dev->dev_addr, ETH_ALEN);
			break;
	};

	return 0;
}
 
int vlan_dev_hard_header(struct sk_buff *skb, struct net_device *dev,
		          unsigned short type, void *daddr, void *saddr,
					 unsigned len)
{
	struct vlan_hdr *vhdr;
	unsigned short veth_TCI = 0;
	int rc = 0;
	int build_vlan_header = 0;
	struct net_device *vdev = dev; /* save this for the bottom of the method */

	printk("%s: skb: %p type: %hx len: %x vlan_id: %hx, daddr: %p\n",
			__FUNCTION__, skb, type, len, VLAN_DEV_INFO(dev)->vlan_id, daddr);

	/* build vlan header only if re_order_header flag is NOT set.  This
	 ** fixes some programs that get confused when they see a VLAN device
	 ** sending a frame that is VLAN encoded (the consensus is that the VLAN
	 ** device should look completely like an Ethernet device when the
	 ** REORDER_HEADER flag is set)   The drawback to this is some extra
	 ** header shuffling in the hard_start_xmit.  Users can turn off this
	 ** REORDER behaviour with the vconfig tool.
	 **/
	build_vlan_header = ((VLAN_DEV_INFO(dev)->flags & 1) == 0);

	if (build_vlan_header) {
		printk("%s:why why ???\n", __FUNCTION__);
		vhdr = (struct vlan_hdr *) skb_push(skb, VLAN_HLEN);
		veth_TCI = VLAN_DEV_INFO(dev)->vlan_id;
/*		veth_TCI |= vlan_dev_get_egress_qos_mask(dev, skb);  */

		vhdr->h_vlan_TCI = htons(veth_TCI);

		if (type != ETH_P_802_3) {
			vhdr->h_vlan_encapsulated_proto = htons(type);
		} else {
			vhdr->h_vlan_encapsulated_proto = htons(len);
		}

		skb->protocol = htons(ETH_P_8021Q);
		skb->nh.raw = skb->data;
	}

	/* Before delegating work to the lower layer, enter our MAC-address */
	if (saddr == NULL)
		saddr = dev->dev_addr;

	dev = VLAN_DEV_INFO(dev)->real_dev;
	if (skb_headroom(skb) < dev->hard_header_len) {
		struct sk_buff *sk_tmp = skb;
		printk("%s:should I do it???\n", __FUNCTION__);
		skb = skb_realloc_headroom(sk_tmp, dev->hard_header_len);
		kfree_skb(sk_tmp);
		if (skb == NULL) {
			struct net_device_stats *stats = vlan_dev_get_stats(vdev);
			stats->tx_dropped++;
			return -ENOMEM;
		}
		VLAN_DEV_INFO(vdev)->cnt_inc_headroom_on_tx++;
		printk("%s: %s: had to grow skb.\n", __FUNCTION__, vdev->name);
	}

	if (build_vlan_header) {
		/* Now make the underlying real hard header */
		rc = dev->hard_header(skb, dev, ETH_P_8021Q, daddr, saddr, len + VLAN_HLEN);

		if (rc > 0) {
			rc += VLAN_HLEN;
		} else if (rc < 0) {
			rc -= VLAN_HLEN;
		}
	} else {
		/* If here, then we'll just make a normal looking ethernet frame,
		 ** but, the hard_start_xmit method will insert the tag (it has to
		 ** be able to do this for bridged and other skbs that don't come
		 ** down the protocol stack in an orderly manner.
		 **/
		rc = dev->hard_header(skb, dev, type, daddr, saddr, len);
	}

	return rc;
}


int vlan_dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct net_device_stats *stats = vlan_dev_get_stats(dev);
	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)(skb->data);

	/* Handle non-VLAN frames if they are sent to us, for example by DHCP.
	 *     *
	 *         * NOTE: THIS ASSUMES DIX ETHERNET, SPECIFICALLY NOT SUPPORTING
	 *             * OTHER THINGS LIKE FDDI/TokenRing/802.3 SNAPs...
	 *                 */

	if (veth->h_vlan_proto != __constant_htons(ETH_P_8021Q)) {
		int orig_headroom = skb_headroom(skb);
		unsigned short veth_TCI;

		/* This is not a VLAN frame...but we can fix that! */
		VLAN_DEV_INFO(dev)->cnt_encap_on_xmit++;

		printk("%s: proto to encap: 0x%hx (hbo)\n",
				__FUNCTION__, htons(veth->h_vlan_proto));
		/* Construct the second two bytes. This field looks something
		 *        * like:
		 *               * usr_priority: 3 bits  (high bits)
		 *                      * CFI       1 bit
		 *                             * VLAN ID   12 bits (low bits)
		 *                                    */
		veth_TCI = VLAN_DEV_INFO(dev)->vlan_id;
	/*	veth_TCI |= vlan_dev_get_egress_qos_mask(dev, skb); */

		skb = __vlan_put_tag(skb, veth_TCI);
		if (!skb) {
			stats->tx_dropped++;
			return 0;
		}

		if (orig_headroom < VLAN_HLEN) {
			VLAN_DEV_INFO(dev)->cnt_inc_headroom_on_tx++;
		}
	}

#ifdef VLAN_DEBUG
	printk(VLAN_DBG "%s: about to send skb: %p to dev: %s\n",
			__FUNCTION__, skb, skb->dev->name);
	printk(VLAN_DBG "  %2hx.%2hx.%2hx.%2xh.%2hx.%2hx %2hx.%2hx.%2hx.%2hx.%2hx.%2hx %4hx %4hx %4hx\n",
			veth->h_dest[0], veth->h_dest[1], veth->h_dest[2], veth->h_dest[3], veth->h_dest[4], veth->h_dest[5],
			veth->h_source[0], veth->h_source[1], veth->h_source[2], veth->h_source[3], veth->h_source[4], veth->h_source[5],
			veth->h_vlan_proto, veth->h_vlan_TCI, veth->h_vlan_encapsulated_proto);
#endif

	stats->tx_packets++; /* for statics only */
	stats->tx_bytes += skb->len;

	skb->dev = VLAN_DEV_INFO(dev)->real_dev;
	dev_queue_xmit(skb);

	return 0;
}

static inline void vlan_destroy_mc_list(struct dev_mc_list *mc_list)
{
	struct dev_mc_list *dmi = mc_list;
	struct dev_mc_list *next;

	while(dmi) {
		next = dmi->next;
		kfree(dmi);
		dmi = next;
	}
} 


static void vlan_flush_mc_list(struct net_device *dev)
{
	struct dev_mc_list *dmi = dev->mc_list;

	while (dmi) {
		printk(KERN_DEBUG "%s: del %.2x:%.2x:%.2x:%.2x:%.2x:%.2x mcast address from vlan interface\n",
				dev->name,
				dmi->dmi_addr[0],
				dmi->dmi_addr[1],
				dmi->dmi_addr[2],
				dmi->dmi_addr[3],
				dmi->dmi_addr[4],
				dmi->dmi_addr[5]);
		dev_mc_delete(dev, dmi->dmi_addr, dmi->dmi_addrlen, 0);
		dmi = dev->mc_list;
	}

	/* dev->mc_list is NULL by the time we get here. */
	vlan_destroy_mc_list(VLAN_DEV_INFO(dev)->old_mc_list);
	VLAN_DEV_INFO(dev)->old_mc_list = NULL;
}


int vlan_dev_open(struct net_device *dev)
{
	   if (!(VLAN_DEV_INFO(dev)->real_dev->flags & IFF_UP))
			      return -ENETDOWN;
		 
		   return 0;
}
 
int vlan_dev_stop(struct net_device *dev)
{
	   vlan_flush_mc_list(dev);
		   return 0;
}
 
int vlan_dev_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
{
	struct net_device *real_dev = VLAN_DEV_INFO(dev)->real_dev;
	struct ifreq ifrr;
	int err = -EOPNOTSUPP;

	strncpy(ifrr.ifr_name, real_dev->name, IFNAMSIZ);
	ifrr.ifr_ifru = ifr->ifr_ifru;
	printk("%s: is called,the cmd is %d\n", __FUNCTION__, cmd);

	switch(cmd) {
		case SIOCGMIIPHY:
		case SIOCGMIIREG:
		case SIOCSMIIREG:
			if (real_dev->do_ioctl && netif_device_present(real_dev))
				err = real_dev->do_ioctl(real_dev, &ifrr, cmd);
			break;

		case SIOCETHTOOL:
			err = dev_ethtool(&ifrr);
	}

	if (!err)
		ifr->ifr_ifru = ifrr.ifr_ifru;

	return err;
}


int vlan_dev_change_mtu(struct net_device *dev, int new_mtu)
{
	/* the later can be added more */
	if (VLAN_DEV_INFO(dev)->real_dev->mtu < new_mtu)
		return -ERANGE;

	dev->mtu = new_mtu;
	return 0;
}


int vlan_dev_set_mac_address(struct net_device *dev, void *addr_struct_p)
{
	/* the later can be added more */
	printk("%s:I do nothing...\n", __FUNCTION__);
	return 0;
}


void vlan_dev_set_multicast_list(struct net_device *vlan_dev)
{
	/* the later can be added more */
	printk("%s:I do nothing...\n", __FUNCTION__);
}