rawtx.c

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

/*
 *  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
#define WITH_KIOBUF

#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;
	unsigned char * dbuffer;
	unsigned int dbuffer_size;
};


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

/* 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 *packetbuf, unsigned int dlen,
                                const struct sockaddr_in *saddr,
                                const struct sockaddr_in *daddr,
                                int do_csum)
{
	struct iphdr *iph = (struct iphdr *) packetbuf;
	struct udphdr *uh = (struct udphdr *) (packetbuf + 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) {
		int cs = csum_partial(packetbuf + IP_HEADER_LEN,
		                      dlen + sizeof(struct udphdr), 0);
		uh->check = csum_tcpudp_magic(iph->saddr, iph->daddr,
		                              dlen + sizeof(struct udphdr),
		                              IPPROTO_UDP, cs);
	}
}


/*
 *  This is called from kfree_skb for the frames we sent through
 *  the mmap interface.
 *  Restore hte sk_buff data pointers and decrement the queue usage count.
 */
static void rawtx_mmap_skb_destructor(struct sk_buff *skb)
{
	struct rawtx_buffer *buf;
	struct rawtx_private_data *priv;

	buf = (struct rawtx_buffer *) (skb->head - sizeof(struct rawtx_buffer));
	priv = (struct rawtx_private_data *) skb->sk;

	/* Restore pointers */
	skb->sk = NULL;
	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;

	/* Decrement usage count */
	up(&priv->queue_sem);

	/* Release buffer to the application */
	buf->busy = 0;
}


/*
 *  This is called from kfree_skb for the frames we sent
 *  through the kiobuf interface.
 *  Decrement the queue usage count.
 */
static void rawtx_kiobuf_skb_destructor(struct sk_buff *skb)
{
	struct rawtx_private_data *priv;
	priv = (struct rawtx_private_data *) skb->sk;
	up(&priv->queue_sem);
}


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

	if (dev == NULL)
		return -EINVAL;

	hhlen = 0;
	if (priv->proto != RAWTX_PROTO_RAW)
		hhlen = priv->dev->hard_header_len;

	nhlen = 0;
	if ( priv->proto == RAWTX_PROTO_UDPIP ||
	     priv->proto == RAWTX_PROTO_UDPIP_NOSUM )
		nhlen = UDP_IP_HEADER_LEN;

	/* Check room reserved for header */
	if (buf->header_len < hhlen + nhlen)
		return -EINVAL;

	/* Check packet length */
	if (buf->data_len + nhlen > dev->mtu)
		return -EMSGSIZE;

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

	/* Allocate a position in the transmission queue */
// Log queue length
//	printk(KERN_NOTICE MODNAME "rawtx_mmap_transmit: queue_sem=%d\n",
//	       atomic_read(&priv->queue_sem.count));
	down(&priv->queue_sem);

	/* Mark buffer as busy */
	buf->busy = 1;

	/* Make skb point to our own data buffer */
	skb->head = buf->buffer;
	skb->data = buf->buffer + buf->header_len;
	skb->tail = buf->buffer + buf->header_len + buf->data_len;
	skb->end = skb->tail;
	skb->len = buf->data_len;
	skb_shinfo(skb)->nr_frags = 0;
	skb_shinfo(skb)->frag_list = NULL;

	skb->sk = priv; /* Can we do this ?? */
	skb->destructor = rawtx_mmap_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,
		                    &priv->srcaddr, &priv->dstaddr,
		                    (priv->proto == RAWTX_PROTO_UDPIP));
	}

	/* Add ethernet header */
	if ( priv->proto != RAWTX_PROTO_RAW ) {
		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);

	return err;
}


/*
 *  Check that the offset points to a valid buffer structure.
 */
static inline int valid_buffer(struct rawtx_private_data *priv,
                               unsigned int offset)
{
	struct rawtx_buffer *buf;
	unsigned int blen = priv->dbuffer_size;
	unsigned int end;

	if (offset > blen ||
	    offset + sizeof(struct rawtx_buffer) > blen)
		return 0;
	buf = (struct rawtx_buffer *) (priv->dbuffer + offset);
	end = offset + buf->header_len;
	if (end < offset || end > blen)
		return 0;
	end += buf->data_len;
	if (end < offset || end > blen)
		return 0;
	end += sizeof(struct skb_shared_info);
	if (end < offset || end > blen)
		return 0;
	return 1;
}


/*
 *  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_buffer(priv, ofs))
		return -EINVAL;

	buf = (struct rawtx_buffer *) (priv->dbuffer + ofs);
	return rawtx_mmap_transmit(priv, buf);
}


/*
 *  For each page in the iobuf, add an extra fragment to the skb.
 */
static void setup_fragments(struct kiobuf *iobuf, struct sk_buff *skb)
{
	skb_frag_t *frags = skb_shinfo(skb)->frags;
	int i;

	skb->len += iobuf->length;
	skb->data_len = iobuf->length;

	for (i = 0; i < iobuf->nr_pages; i++) {
		struct page *pag = iobuf->maplist[i];

		if (i >= MAX_SKB_FRAGS) {
			printk(KERN_ERR MODNAME "rhaa, MAX_SKB_FRAGS exceeded\n");
			break;
		}

		get_page(pag);	/* lock page */
		frags[i].page = pag;
		frags[i].page_offset = 0;
		frags[i].size = PAGE_SIZE;
	}

	/* Store nr of fragments */
	skb_shinfo(skb)->nr_frags = i;

	/* Fix first and last fragments */
	if (i > 0) {
		unsigned int s;
		frags[0].page_offset = iobuf->offset;
		s = (iobuf->offset + iobuf->length) % PAGE_SIZE;
		if (s == 0) s = PAGE_SIZE;
		frags[i-1].size = s;
		frags[0].size -= iobuf->offset;
	}
}


/*
 *  Send specified buffer to network.
 */
static int rawtx_write(struct file *filp, const char *buf, size_t count,
                       loff_t *pos)
{
	struct rawtx_private_data *priv = filp->private_data;
	struct net_device *dev = priv->dev;
	struct sk_buff *skb;
	struct kiobuf *iobuf;
	int hhlen, nhlen;
	int err;

#ifdef WITH_KIOBUF
	/* Cannot do checksumming with kiobufs yet */
	if (priv->proto == RAWTX_PROTO_UDPIP)
		return -EINVAL;
#endif

	/* Check valid device selected */
	if (dev == NULL)