rawtx.c.3

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

	return rawtx_do_transmit(priv, buf);
}


/*
 *  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->head;
	up(&priv->queue_sem);
}


/*
 *  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;

	/* Check valid device selected */
	if (dev == NULL)
		return -EINVAL;

	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 packet length */
	if (count + nhlen > dev->mtu)
		return -EMSGSIZE;

	/* Allocate sk_buff */
	skb = alloc_skb(hhlen + nhlen + 4, GFP_KERNEL);
	if (skb == NULL) {
		printk(KERN_ERR MODNAME "alloc_skb failed\n");
		return -ENOBUFS;
	}

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

	/* Fill in skb structure */
	*(struct rawtx_private_data **)skb->head = priv;
	skb->sk = NULL;
	skb->destructor = rawtx_kiobuf_skb_destructor;
	skb->priority = 0;
	skb->dst = NULL;
	skb->dev = dev;
	skb->protocol = htons(ETH_P_IP);
	skb->ip_summed = CHECKSUM_NONE;
	skb_reserve(skb, hhlen + nhlen + 4);

	/* Only one thread at a time may use the global kiobuf */
	down(&global_iobuf_mutex);
	iobuf = global_iobuf;

	/* Map data in the kiobuf */
	err = map_user_kiobuf(WRITE, iobuf, (unsigned long) buf, count);
	if (err) {
		printk(KERN_ERR MODNAME "map_user_kiobuf failed");
		up(&global_iobuf_mutex);
		kfree(skb);
		return err;
	}

	/* Setup packet fragment(s) */
	setup_fragments(iobuf, skb);

	/* Release kiobuf */
	unmap_kiobuf(iobuf);
	iobuf = NULL;
	up(&global_iobuf_mutex);

	/* Build 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, count, skb,
		                    &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, nhlen + count);
	if (err < 0) {
		printk(KERN_ERR MODNAME "hard_header failed");
		kfree_skb(skb);
		return err;
	}

	printk(KERN_NOTICE MODNAME "rawtx_write: nr_frags=%d, len=%d, data_len=%d, frags[0].size=%d, frags[0].page=%p\n",
	       skb_shinfo(skb)->nr_frags, skb->len, skb->data_len,
	       skb_shinfo(skb)->frags[0].size, page_address(skb_shinfo(skb)->frags[0].page));

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

	return err;
}


/*
 *  Modify the queue length limit.
 *  This is not thread safe.
 */
static int set_tx_queue_len(struct rawtx_private_data *priv, int newlen)
{
	while (priv->queue_len > newlen) {
		down(&priv->queue_sem);
		priv->queue_len--;
	}

	while (priv->queue_len < newlen) {
		up(&priv->queue_sem);
		priv->queue_len++;
	}

	return 0;
}


static int rawtx_ioctl(struct inode *inode, struct file *filp,
                       unsigned int cmd, unsigned long arg)
{
	struct rawtx_private_data *priv = filp->private_data;
	char devicename[IFNAMSIZ];
	struct net_device *dev;
	int err;

	switch (cmd) {
	  case RAWTX_IOCTL_SETDEVICE:
		err = strncpy_from_user(devicename, (char *) arg, sizeof(devicename));
		if (err < 0)
			return err;
		dev = dev_get_by_name(devicename);
		if (dev == NULL)
			return -ENOENT;
		if (priv->dev != NULL)
			dev_put(priv->dev);
		priv->dev = dev;
		memcpy(priv->saddr, dev->dev_addr, sizeof(priv->saddr));
		return 0;

	  case RAWTX_IOCTL_SETPROTO:
	  	if ( arg != RAWTX_PROTO_ETHERNET &&
	  	     arg != RAWTX_PROTO_UDPIP &&
	  	     arg != RAWTX_PROTO_UDPIP_NOSUM )
	  		return -EINVAL;
	  	priv->proto = arg;
	  	return 0;

	  case RAWTX_IOCTL_SETSADDR:
		if ( copy_from_user(priv->saddr, (void *) arg, sizeof(priv->saddr)) )
			return -EFAULT;
		return 0;

	  case RAWTX_IOCTL_SETDADDR:
		if ( copy_from_user(priv->daddr, (void *) arg, sizeof(priv->daddr)) )
			return -EFAULT;
		return 0;

	  case RAWTX_IOCTL_SETSRCADDR:
	  	if ( copy_from_user(&priv->srcaddr, (void *) arg, sizeof(priv->srcaddr)) )
	  		return -EFAULT;
	  	return 0;

	  case RAWTX_IOCTL_SETDSTADDR:
	  	if ( copy_from_user(&priv->dstaddr, (void *) arg, sizeof(priv->dstaddr)) )
	  		return -EFAULT;
	  	return 0;

	  case RAWTX_IOCTL_SETQUEUELEN:
	  	if (arg < 1 || arg > MAX_TXQUEUE_LEN)
	  		return -EINVAL;
	  	return set_tx_queue_len(priv, arg);

	  case RAWTX_IOCTL_GETBUFFER:
	  	{ unsigned int ofs;
	  	  int err;
	  	  if ( (err = rawtx_get_buffer(priv, &ofs)) == 0 &&
	  	       (err = put_user(ofs, (unsigned int *) arg)) != 0 )
	  	  	rawtx_release_buffer(priv, ofs);
	  	  return err; }

	  case RAWTX_IOCTL_SENDBUFFER:
	  	return rawtx_send_buffer(priv, arg);
	}

	return -EINVAL;
}


/*
 *  Map skb buffer pool into process memory space.
 */
static int rawtx_mmap(struct file *filp, struct vm_area_struct *vma)
{
	unsigned long start = (unsigned long) vma->vm_start;
	unsigned long size = (unsigned long) (vma->vm_end - vma->vm_start);
	unsigned long page, pos, err;

	printk(KERN_INFO MODNAME "rawtx_mmap()\n");

	/* Check mmap size */
	if (size != RAWTX_BUFPOOL_SIZE)
		return -EINVAL;

	/* Loop through buffer pages and map them into process space */
	for (pos = 0; pos < size; pos += PAGE_SIZE) {
		page = virt_to_phys(((void *)dbuffer) + pos);
		err = remap_page_range(start + pos, page, PAGE_SIZE, PAGE_SHARED);
		if (err < 0)
			return err;
	}

	return 0;
}


static int rawtx_open(struct inode *inode, struct file *filp)
{
	struct rawtx_private_data *priv;

	priv = kmalloc(sizeof(struct rawtx_private_data), GFP_KERNEL);
	if (priv == NULL) {
		printk(KERN_ERR MODNAME "Cannot allocate rawtx_private_data\n");
		return -ENOMEM;
	}
	filp->private_data = priv;

	priv->dev = NULL;
	priv->proto = RAWTX_PROTO_ETHERNET;
	priv->queue_len = 1;
	sema_init(&priv->queue_sem, priv->queue_len);

	MOD_INC_USE_COUNT;

	return 0;
}


/*
 *  Release all buffers used by this process, free private data,
 *  update module use count.
 *  Called when process closes the device.
 */
static int rawtx_release(struct inode *inode, struct file *filp)
{
	struct rawtx_private_data *priv = filp->private_data;
	int i;

	for (i = 0; i + RAWTX_BUF_SIZE <= RAWTX_BUFPOOL_SIZE; i += RAWTX_BUF_SIZE) {
		struct rawtx_buffer *buf = (struct rawtx_buffer *) (dbuffer + i);
		if (atomic_read(&buf->usecount) > 0 && buf->used_by == priv) {
			atomic_set(&buf->usecount, 0);
			buf->used_by = NULL;
			freelist_add_buffer(buf);
		}
	}

	if (priv->dev != NULL)
		dev_put(priv->dev);
	kfree(priv);

	MOD_DEC_USE_COUNT;

	return 0;
}


/* Device file operations structure */
static struct file_operations rawtx_fops = {
	write: rawtx_write,
	ioctl: rawtx_ioctl,
	mmap: rawtx_mmap,
	open: rawtx_open,
	release: rawtx_release
};


int rawtx_init(void)
{
	mem_map_t *page, *lastpage;
	int err;

	printk(KERN_INFO MODNAME "Initializing.\n");

	/* Allocate kiobuf */
	if ( alloc_kiovec(1, &global_iobuf) ) {
		printk(KERN_ERR MODNAME "Cannot allocate kiobuf\n");
		return -ENOMEM;
	}

	/* Allocate master sk_buff */
	master_skb = alloc_skb(RAWTX_BUF_SIZE, GFP_KERNEL);
	if (master_skb == NULL) {
		printk(KERN_ERR MODNAME "Cannot allocate master skb.\n");
		free_kiovec(1, &global_iobuf);
		return -ENOBUFS;
	}

	/* Allocate data buffer pool */
	dbuffer = kmalloc(RAWTX_BUFPOOL_SIZE, GFP_KERNEL);
	if (dbuffer == NULL) {
		printk(KERN_ERR MODNAME "Cannot allocate skb buffer pool.\n");
		kfree_skb(master_skb);
		free_kiovec(1, &global_iobuf);
		return -ENOMEM;
	}

	/* Loop through buffer pages and lock them in main memory */
	lastpage = virt_to_page(dbuffer + RAWTX_BUFPOOL_SIZE);
	for (page = virt_to_page(dbuffer); page < lastpage; page++)
		mem_map_reserve(page);

	/* Register character device */
	err = register_chrdev(RAWTX_MAJOR, "rawtx", &rawtx_fops);
	if (err < 0) {
		printk(KERN_ERR MODNAME "Cannot register character device %d.\n", RAWTX_MAJOR);
		kfree(dbuffer);
		kfree_skb(master_skb);
		free_kiovec(1, &global_iobuf);
		return err;
	}

	freelist_init();

	printk(KERN_INFO MODNAME "Ready.\n");
	return 0;
}


void rawtx_cleanup(void)
{
	printk(KERN_INFO MODNAME "Cleaning up.\n");

	unregister_chrdev(RAWTX_MAJOR, "rawtx");
	kfree(dbuffer);
	kfree_skb(master_skb);
	free_kiovec(1, &global_iobuf);

	printk(KERN_INFO MODNAME "Done.\n");
}

module_init(rawtx_init);
module_exit(rawtx_cleanup);

/* end */