rawtx.c.3

实现一个网卡到内存空间的零拷贝程序

/*
 *  rawtx.c
 *  Joris van Rantwijk, May 2001.
 *
 *  Desperate attempt to implement a quick-and-dirty zero-copy
 *  networking interface in the Linux kernel.
 *
 *  Userspace mmap taken from example http://kernelnewbies.org/code/mmap/
 */

#define __KERNEL__
#define MODULE

#include <linux/stddef.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/mm.h>
#include <linux/wrapper.h>
#include <linux/malloc.h>
#include <linux/fs.h>
#include <linux/errno.h>
#include <linux/types.h>
#include <linux/unistd.h>
#include <linux/skbuff.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include <linux/in.h>
#include <linux/in6.h>
#include <linux/if_ether.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/spinlock.h>
#include <linux/iobuf.h>
#include <asm/uaccess.h>
#include <asm/io.h>
#include <asm/checksum.h>
#include <asm/semaphore.h>
#include "rawtx.h"


#define MODNAME		"rawtx: "

#define ETH_ADDRESS_LEN	6
#define IP_HEADER_LEN		(5 * 4)
#define UDP_IP_HEADER_LEN	(IP_HEADER_LEN + sizeof(struct udphdr))
#define IP_DEFAULT_TTL		64

#define MAX_TXQUEUE_LEN		128


/* Private data in the device file descriptor */
struct rawtx_private_data {
	struct net_device *dev;
	int proto;
	unsigned char saddr[ETH_ADDRESS_LEN];
	unsigned char daddr[ETH_ADDRESS_LEN];
	struct sockaddr_in srcaddr, dstaddr;
	struct semaphore queue_sem;
	int queue_len;
};


/* Pre-allocated kiobuf for mapping process memory */
static struct kiobuf *global_iobuf;
static DECLARE_MUTEX(global_iobuf_mutex);

/* Pointer to skb buffer pool */
static unsigned char * dbuffer;

/* Pointer to list of free buffers */
static struct rawtx_buffer * freelist;

/* Spinlock to protect the freelist */
static spinlock_t freelist_lock = SPIN_LOCK_UNLOCKED;

/* Wait on this list to get a free buffer */
static DECLARE_WAIT_QUEUE_HEAD(freelist_waitqueue);

/* Master sk_buff, all others are cloned from this one. */
static struct sk_buff * master_skb;


/*
 *  Construct the IP and UDP headers for an UDP/IP datagram.
 *  packetbuf points to the start of the resulting packet with
 *  exactly UPD_IP_HEADER_LEN bytes room to put the header, followed
 *  by dlen bytes of application data.
 *  Fragmentation is not supported, the datagram must fit in a single packet.
 */
static void build_udp_ip_header(unsigned char *hdr, unsigned int dlen,
                                const struct sk_buff *skb,
                                const struct sockaddr_in *saddr,
                                const struct sockaddr_in *daddr,
                                int do_csum)
{
	struct iphdr *iph = (struct iphdr *) hdr;
	struct udphdr *uh = (struct udphdr *) (hdr + IP_HEADER_LEN);
	static int ip_ident_hack = 1;

	/* Build IP header */
	iph->version = 4;
	iph->ihl = 5;
	iph->tos = 0;
	iph->tot_len = htons(dlen + UDP_IP_HEADER_LEN);
	iph->id = htons(ip_ident_hack++); /* Ugly */
	iph->frag_off = 0;
	iph->ttl = IP_DEFAULT_TTL;
	iph->protocol = IPPROTO_UDP;
	iph->saddr = saddr->sin_addr.s_addr;
	iph->daddr = daddr->sin_addr.s_addr;
	iph->check = 0;
	iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);

	/* Build UPD header */
	uh->source = saddr->sin_port;
	uh->dest = daddr->sin_port;
	uh->len = htons(dlen + sizeof(struct udphdr));
	uh->check = 0;
	if (do_csum) {
/* disabled
		int i;
		int cs = csum_partial(uh, skb->len - IP_HEADER_LEN, 0);
	also do checksum of fragments here ...
		uh->check = csum_tcpudp_magic(iph->saddr, iph->daddr,
		                              dlen + sizeof(struct udphdr),
		                              IPPROTO_UDP, cs);
*/
	}
}


/*
 *  Check that this is a valid offset of a buffer in the pool.
 */
static inline int valid_offset(unsigned int ofs)
{
	return (ofs + RAWTX_BUF_SIZE <= RAWTX_BUFPOOL_SIZE &&
	        ofs % RAWTX_BUF_SIZE == 0);
}


/*
 *  Get a rawtx_buffer from the list of free buffers.
 *  Return a pointer to the buffer, or NULL if the free list is
 *  currently empty.
 */
static inline struct rawtx_buffer * freelist_get_buffer(void)
{
	struct rawtx_buffer *buf;
	unsigned long flags;
	spin_lock_irqsave(&freelist_lock, flags);
	buf = freelist;
	if (buf != NULL) {
		freelist = buf->nextfree;
		buf->nextfree = NULL;
	}
	spin_unlock_irqrestore(&freelist_lock, flags);
	return buf;
}


/*
 *  Put a rawtx_buffer on the list of free buffers.
 *  If the freelist was previously empty, we wake up the
 *  freelist waitqueue.
 */
static inline void freelist_add_buffer(struct rawtx_buffer *buf)
{
	int need_wakeup;
	unsigned long flags;
	spin_lock_irqsave(&freelist_lock, flags);
	need_wakeup = (freelist == NULL);
	buf->nextfree = freelist;
	freelist = buf;
	spin_unlock_irqrestore(&freelist_lock, flags);
	if (need_wakeup)
		wake_up(&freelist_waitqueue);
}

 
/*
 *  Sleep until a free buffer becomes available.
 *  We don't actually guarantee that a free buffer is available
 *  when we return...
 */
static inline void freelist_wait(void)
{
	DECLARE_WAITQUEUE(wait, current);
	set_current_state(TASK_INTERRUPTIBLE);
	add_wait_queue(&freelist_waitqueue, &wait);
	if (freelist != NULL)
		set_current_state(TASK_RUNNING);
	schedule();
	remove_wait_queue(&freelist_waitqueue, &wait);
}


/*
 *  Partition the buffer pool into individual buffers, and put
 *  them all on the freelist.
 */
static void freelist_init(void)
{
	int i;
	freelist = (struct rawtx_buffer *) dbuffer;
	for (i = 0; i + RAWTX_BUF_SIZE <= RAWTX_BUFPOOL_SIZE; i += RAWTX_BUF_SIZE){
		struct rawtx_buffer *buf = (struct rawtx_buffer *) (dbuffer + i);
		buf->nextfree = (i + 2*RAWTX_BUF_SIZE <= RAWTX_BUFPOOL_SIZE) ?
		  (struct rawtx_buffer *) (dbuffer + i + RAWTX_BUF_SIZE) :
		  NULL;
		buf->used_by = 0;
		atomic_set(&buf->usecount, 0);
	}
}


/*
 *  Put the data buffer on the free list.
 *  This is called from kfree_skb.
 */
static void rawtx_skb_destructor(struct sk_buff *skb)
{
	unsigned int ofs;
	struct rawtx_buffer *buf;

	ofs = (skb->head - dbuffer) - sizeof(struct rawtx_buffer);
	if (!valid_offset(ofs)) {
		printk(KERN_ERR MODNAME "rawtx_skb_destructor: rhaa, not our skb !! skb=%p skb->head=%p\n",
		       skb, skb->head);
		return;
	}

	buf = (struct rawtx_buffer *) (dbuffer + ofs);

	/* Restore pointers */
	skb->head = master_skb->head;
	skb->data = master_skb->data;
	skb->tail = master_skb->tail;
	skb->end = master_skb->end;
	skb->len = master_skb->len;

	/* Release data buffer */
	if (atomic_dec_and_test(&buf->usecount)) {
		freelist_add_buffer(buf);
	}
}


static int rawtx_do_transmit(struct rawtx_private_data *priv,
                             struct rawtx_buffer *buf)
{
	unsigned int ofs;
	struct net_device *dev = priv->dev;
	struct sk_buff *skb;
	int hhlen;
	int err;

	if (dev == NULL)
		return -EINVAL;
	hhlen = priv->dev->hard_header_len;
	if ( priv->proto == RAWTX_PROTO_UDPIP ||
	     priv->proto == RAWTX_PROTO_UDPIP_NOSUM )
		hhlen += UDP_IP_HEADER_LEN;

	/* Check buf valitidy */
	ofs = ((unsigned char *) buf) - dbuffer;
	if ( buf->data_ofs - ofs < sizeof(struct rawtx_buffer) + hhlen ||
	     buf->data_ofs - ofs > RAWTX_BUF_SIZE - sizeof(struct skb_shared_info) ||
	     buf->data_ofs - ofs + buf->data_len > RAWTX_BUF_SIZE - sizeof(struct skb_shared_info) )
		return -EINVAL;

	/* Check packet length */
	if (buf->data_len > dev->mtu ||
	    ( (priv->proto == RAWTX_PROTO_UDPIP ||
	       priv->proto == RAWTX_PROTO_UDPIP_NOSUM) &&
	      buf->data_len + UDP_IP_HEADER_LEN > dev->mtu) )
		return -EMSGSIZE;

	/* Allocate sk_buff */
	skb = skb_clone(master_skb, GFP_KERNEL);
	if (skb == NULL)
		return -ENOBUFS;

	/* Increment usecount */
	atomic_inc(&buf->usecount);

	/* Make skb point to our own data buffer */
	skb->head = dbuffer + ofs + sizeof(struct rawtx_buffer);
	skb->data = dbuffer + buf->data_ofs;
	skb->tail = dbuffer + buf->data_ofs + buf->data_len;
	skb->end = dbuffer + ofs + RAWTX_BUF_SIZE - sizeof(struct skb_shared_info);
	skb->len = buf->data_len;
	skb_shinfo(skb)->nr_frags = 0;
	skb_shinfo(skb)->frag_list = NULL;

	skb->destructor = rawtx_skb_destructor;
	skb->priority = 0;
	skb->dst = NULL;
	skb->dev = dev;
	skb->protocol = htons(ETH_P_IP);
	skb->ip_summed = CHECKSUM_NONE;

	/* Add IP header */
        if ( priv->proto == RAWTX_PROTO_UDPIP ||
             priv->proto == RAWTX_PROTO_UDPIP_NOSUM ) {
	        skb_push(skb, UDP_IP_HEADER_LEN);
		build_udp_ip_header(skb->data, buf->data_len, NULL,
		                    &priv->srcaddr, &priv->dstaddr,
		                    (priv->proto == RAWTX_PROTO_UDPIP));
	}

	/* Add ethernet header */
	err = dev->hard_header(skb, dev, ntohs(skb->protocol),
	                       priv->daddr, priv->saddr, skb->len);
	if (err < 0) {
		kfree_skb(skb);
		return err;
	}

	/* Send packet */
	err = dev_queue_xmit(skb);

	if (err == 0) {
		/* Release buffer */
		buf->used_by = NULL;
		if (atomic_dec_and_test(&buf->usecount)) {
			freelist_add_buffer(buf);
		}
	}

	return err;
}


/*
 *  Get a free buffer and store the offset in *ofs.
 *  Block until a free buffer is available.
 */
static int rawtx_get_buffer(struct rawtx_private_data *priv, unsigned int *ofs)
{
	struct rawtx_buffer *b;
	int hhlen;

	if (priv->dev == NULL)
		return -EINVAL;

	while ((b = freelist_get_buffer()) == NULL) {
		freelist_wait();
		if ( signal_pending(current) )
			return -ERESTARTSYS;
	}

	b->used_by = priv;
	atomic_set(&b->usecount, 1);
	hhlen = priv->dev->hard_header_len;
	if ( priv->proto == RAWTX_PROTO_UDPIP ||
	     priv->proto == RAWTX_PROTO_UDPIP_NOSUM )
		hhlen += UDP_IP_HEADER_LEN;
	b->data_ofs = ((unsigned char *) b) - dbuffer +
	              sizeof(struct rawtx_buffer) + hhlen;
	b->data_len = RAWTX_BUF_SIZE - sizeof(struct rawtx_buffer) -
	              sizeof(struct skb_shared_info) - hhlen;

	*ofs = ((unsigned char *) b) - dbuffer;

	return 0;
}


/*
 *  Release the previously allocated buffer corresponding to ofs.
 */
static int rawtx_release_buffer(struct rawtx_private_data *priv, unsigned int ofs)
{
	struct rawtx_buffer *buf;

	if (! valid_offset(ofs))
		return -EINVAL;

	buf = (struct rawtx_buffer *) (dbuffer + ofs);
	if (atomic_read(&buf->usecount) == 0 || buf->used_by != priv)
		return -EACCES;

	buf->used_by = NULL;
	if (atomic_dec_and_test(&buf->usecount))
		freelist_add_buffer(buf);

	return 0;
}


/*
 *  Send the buffer corresponding to ofs.
 *  If the call succeeds, the buffer will be released by the driver.
 */
static int rawtx_send_buffer(struct rawtx_private_data *priv, unsigned int ofs)
{
	struct rawtx_buffer *buf;

	if (! valid_offset(ofs))
		return -EINVAL;

	buf = (struct rawtx_buffer *) (dbuffer + ofs);
	if (atomic_read(&buf->usecount) == 0 || buf->used_by != priv)
		return -EACCES;