rtsock.c

fsmlabs的real time linux的内核

/*
 * rtsock.c - version 1.0
 *
 * Written by Robert Kavaler, 1998-2002
 *
 * Copyright (C) 1998-2002, Innomedia, Inc.
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 
 * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer. 
 *
 * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials
 * provided with the distribution. 
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS
 * AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
 * THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <linux/module.h>
#include <linux/version.h>

#include <linux/sched.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/fs.h>
#include <linux/errno.h>
#include <linux/types.h>
#include <linux/proc_fs.h>
#include <linux/fcntl.h>
#include <linux/pci.h>
#include <linux/ioctl.h>
#include <linux/mm.h>
#include <linux/socket.h>
#include <linux/file.h>

#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/ip.h>
#include <linux/tcp.h>
#include <linux/skbuff.h>
#include <linux/udp.h>

#include <net/ip.h>

#include <asm/system.h>
#include <asm/uaccess.h>
#include <asm/io.h>
#include <asm/semaphore.h>
#include <asm/checksum.h>

#include <rtl_core.h>

#include <rtsock.h>

MODULE_AUTHOR("(C) Robert Kavaler, 1998-2003");
MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("RT-Socket core module");

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,4,4)
#define	skb_used	used
#else
#define	skb_used	__unused
#endif

#define	SKB_HEADER_LENGTH	(16+60+8)

#include "rtfifo.h"

#define	rtf_create(m, s)	((m=rtfifo_create(s))? 0 : -1)
#define	rtf_destroy(m)		rtfifo_destroy(m)
#define	rtf_put(m, b, c)	rtfifo_put(m, b, c, 1)
#define	rtf_get(m, b, c)	rtfifo_get(m, b, c, 1)
typedef struct RTFIFO_STRUCT 	*RTFIFO;
#define	RTFIFO_NULL		NULL

struct rt_loop {
	RTFIFO fifo;
	RTFIFO returnFifo;
	int running;
	int reentered;
	int lostPackets;
	atomic_t count;
	int max_count;
};

typedef struct RtSock {
	struct rt_loop toRT;
	struct rt_loop fromRT;
	int skbLength;
	int nSkbs;
	spinlock_t dequeueSpinlock;
	struct tq_struct fromRTTask;
	void (*default_data_ready)(struct sock *, int);
	void (*rtsock_data_ready)(struct sock *, int);
	int   (*callback)(void *, struct sock *, unsigned long);
	void *callbackData;
} RtSock;

static void rtsock_data_ready(RtSock *r, struct sock *sk, int len);

static RtSock   rtsock_devices[RTSOCK_NDEVS];
static int	rtsock_major = RTSOCK_MAJOR;
static int	rtsock_ndevs = 1;
static int	rtsock_irq = 0;

static void
rtsock_data_ready0(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[0], sk, len);
}

static void
rtsock_data_ready1(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[1], sk, len);
}

static void
rtsock_data_ready2(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[2], sk, len);
}

static void
rtsock_data_ready3(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[3], sk, len);
}

static void
rtsock_data_ready4(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[4], sk, len);
}

static void
rtsock_data_ready5(struct sock *sk, int len)
{
	rtsock_data_ready(&rtsock_devices[5], sk, len);
}

static void (*rtsock_data_ready_list[RTSOCK_NDEVS])(struct sock *, int) = {
	rtsock_data_ready0,
	rtsock_data_ready1,
	rtsock_data_ready2,
	rtsock_data_ready3,
	rtsock_data_ready4,
	rtsock_data_ready5,
};

static struct sock *
GetSockFromFd(int fd, int *errp)
{
	struct file *fp;
	struct inode *inode;

	fp = fcheck(fd);
	if(!fp) {
		*errp = -EBADF;
		return NULL;
	}
	inode = fp->f_dentry->d_inode;
        if(!inode->i_sock) {
		*errp = -ENOTSOCK;
		return NULL;
	}
	return inode->u.socket_i.sk;
}

static int
rtsock_put_alloc_skb(RtSock *r)
{
	struct sk_buff *skb;

	skb = alloc_skb(r->skbLength+SKB_HEADER_LENGTH, GFP_ATOMIC);
	if(!skb) {
		printk("rtsock: lost packet\n");
		r->fromRT.lostPackets++;
		return -1;
	}
	skb_reserve(skb, SKB_HEADER_LENGTH);
	skb->skb_used = 0;
	rtf_put(r->fromRT.returnFifo, &skb, sizeof(skb));
	return 0;
}

static void
append_and_send_udp(struct sk_buff *skb)
{
	struct udphdr *u;
	int checksum;

	checksum = csum_partial((char *) skb_push(skb, 0), skb->len, 0);
	u = (struct udphdr *) skb_push(skb, sizeof(struct udphdr));
	u->source = skb->sk->sport;
	u->dest = skb->sk->dport;
	u->len = htons(skb->len);
	u->check = 0;
	checksum = csum_partial((char *) u, sizeof(struct udphdr), checksum);
	u->check = csum_tcpudp_magic(skb->sk->saddr, skb->sk->daddr, skb->len,
		IPPROTO_UDP, checksum);
	ip_queue_xmit(skb);
}


static void
rtsock_task(void *data)
{
	RtSock *r = data;
	struct sk_buff *skb;
	struct sock *sk;

	r->fromRT.running = 0;
	while(rtf_get(r->fromRT.fifo, &skb, sizeof(skb)) == sizeof(skb)) {
		switch(skb->skb_used) {
		    case 0:
			append_and_send_udp(skb);
			rtsock_put_alloc_skb(r);
			break;
		    case 2:
			kfree_skb(skb);
			rtsock_put_alloc_skb(r);
			break;
		    case 1:
			// don't allow rt process to send skb previously rcv'd
		    case 3:
			atomic_inc(&r->toRT.count);
			sk = skb->sk;
			kfree_skb(skb);

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,3,0)
			sock_put(sk);
#endif
			break;
		    default:
			printk("rtsock: received weird skb (used=%d)\n", 
				skb->skb_used);
			break;
		}
	}
}

static void 
rtsock_irq_handler(int irq, void *dev_id, struct pt_regs *p)
{
	RtSock *r;
	int  minor;

	for(minor=0; minor<rtsock_ndevs; minor++) {
		r = &rtsock_devices[minor];
		if(test_and_set_bit(0, (void *)&r->fromRT.running)) {
			return;
		}
		r->fromRTTask.routine = rtsock_task;
		r->fromRTTask.data = r;
		queue_task(&r->fromRTTask, &tq_immediate);
		mark_bh(IMMEDIATE_BH);
	}
}

static void
rtsock_data_ready(RtSock *r, struct sock *sk, int len)
{
	struct sk_buff *skb;

	if(!r) {
		return;
	}
					// insure only one dequeuer
	write_lock(&r->dequeueSpinlock);
					// look at linux/net/core/datagram.c
	while((skb = skb_dequeue(&sk->receive_queue))) {
		sk->stamp = skb->stamp;
		if(atomic_add_negative(-1, &r->toRT.count)) {
			atomic_inc(&r->toRT.count);
			printk("dropping skb\n");
			r->toRT.lostPackets++;
			kfree_skb(skb);
		} else {
			skb->skb_used = 1;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,3,0)
			sock_hold(sk);
#endif
			rtf_put(r->toRT.fifo, &skb, sizeof(skb));
		}
	}
	write_unlock(&r->dequeueSpinlock);
}


static int 
rtsock_open(struct inode *inode, struct file *filp)
{
	int minor = inode->i_rdev & 0xFF;
	RtSock *r;

	if(minor >= rtsock_ndevs) return -ENODEV;
	r = &rtsock_devices[minor];
	filp->private_data = r;
	r->default_data_ready = NULL;
	MOD_INC_USE_COUNT;
	return 0;
}

static int 
rtsock_release(struct inode *inode, struct file *filp)
{
	MOD_DEC_USE_COUNT;
	return 0;
}

static int 
rtsock_initialize(RtSock *r, int minor)
{
	r->fromRT.fifo = RTFIFO_NULL;
	r->toRT.fifo = RTFIFO_NULL;
	r->rtsock_data_ready = rtsock_data_ready_list[minor];
	spin_lock_init(&r->dequeueSpinlock);
	return 0;
}

static void 
rtsock_deinitialize(RtSock *r)
{
}

static ssize_t
rtsock_read(struct file *filp, char *buf, size_t count, loff_t *new)
{
	return 0;
}

static ssize_t
rtsock_write(struct file *filp, const char *buf, size_t count, loff_t *new)
{
	return 0;
}

static loff_t
rtsock_lseek(struct file *filp, loff_t off, int whence)
{
	return 0;
}

static int
rtsock_ioctl(struct inode *inode, struct file *filp, 
	unsigned int cmd, unsigned long arg)
{
	int *intp = (int *) arg;
	RtSock *r = filp->private_data;
	struct sock *sock;
	int  fd, x;
	int  size = _IOC_SIZE(cmd);
	int  ret = 0;


	if(_IOC_TYPE(cmd) != RTSOCK_IOC_MAGIC) return -EINVAL;

	if(_IOC_DIR(cmd) & _IOC_READ) {
		ret = access_ok(VERIFY_WRITE, (void *)arg, size);
	} else if(_IOC_DIR(cmd) & _IOC_WRITE) {
		ret = access_ok(VERIFY_READ, (void *)arg, size);
	}
	if(!ret) return -EFAULT;

	switch(cmd) {
	case RTSOCK_IOCREDIRECTFD:
		__get_user(fd, intp);
		sock = GetSockFromFd(fd, &ret);
		if(!sock) {
			ret = -EINVAL;
			break;
		}
		if(r->default_data_ready == NULL) {
			r->default_data_ready = sock->data_ready;
		}
		sock->data_ready = r->rtsock_data_ready;
		break;
	case RTSOCK_IOCRESETFD:
		__get_user(fd, intp);
		sock = GetSockFromFd(fd, &ret);
		if(!sock || !r->default_data_ready) {
			ret = -EINVAL;
			break;
		}
		if(r->default_data_ready) {
			sock->data_ready = r->default_data_ready;
		}
		break;
	case RTSOCK_IOCGETSOCKADDR:
		__get_user(fd, intp);
		sock = GetSockFromFd(fd, &ret);
		if(!sock) {
			ret = -EINVAL;
			break;
		}
		x = (int) sock;
		__put_user(x, intp);
		break;
	case RTSOCK_IOCINTERFACE:
		__get_user(fd, intp);
		sock = GetSockFromFd(fd, &ret);
		if(!sock) {
			ret = -EINVAL;
			break;
		}
		ret = (r->callback)? (r->callback)(r->callbackData, sock, arg)
			: -EINVAL;
		break;
	default:
		ret = -EINVAL;
		break;
	}
	return ret;
}

RtSock *
rtsock_create(int minor, int fromRTmax_count, int toRTmax_count, int skbLength)
{
	RtSock *r = &rtsock_devices[minor];
	int a, b, c, i;

	r->fromRT.max_count = fromRTmax_count;
	r->toRT.max_count = toRTmax_count;
	atomic_set(&r->toRT.count, toRTmax_count);
	r->skbLength = skbLength;


	a = rtf_create(r->fromRT.fifo, 
	    (r->fromRT.max_count+r->toRT.max_count)*sizeof(struct sk_buff *));
	b = rtf_create(r->fromRT.returnFifo, 
	    r->fromRT.max_count*sizeof(struct sk_buff *));
	c = rtf_create(r->toRT.fifo, 
	    r->toRT.max_count*sizeof(struct sk_buff *));
	if(a < 0 || b < 0 || c < 0) {
		if(a >= 0) rtf_destroy(r->fromRT.fifo);
		if(b >= 0) rtf_destroy(r->fromRT.returnFifo);
		if(c >= 0) rtf_destroy(r->toRT.fifo);
		return NULL;
	}
	for(i=0; i<r->fromRT.max_count; i++) {
		rtsock_put_alloc_skb(r);
	}
	return r;
}

void
rtsock_register_callback(RtSock *r, 
	int (*callback)(void *, struct sock *, unsigned long),
	void *callbackData)
{
	r->callbackData = callbackData;
	r->callback = callback;
}

void
rtsock_destroy(RtSock *r)
{
	struct sk_buff *skb;

	while((skb = rtsock_dequeue_skb(r))) {
		kfree_skb(skb);
	}
	while((skb = rtsock_alloc_skb(r))) {
		kfree_skb(skb);
	}
	rtf_destroy(r->fromRT.fifo);
	rtf_destroy(r->fromRT.returnFifo);
	rtf_destroy(r->toRT.fifo);
}

struct sk_buff *
rtsock_alloc_skb(RtSock *r)
{
	struct sk_buff *skb;

	if(rtf_get(r->fromRT.returnFifo, &skb, sizeof(skb)) != sizeof(skb)) {
		return NULL;
	}
	return skb;

}

struct sk_buff *
rtsock_dequeue_skb(RtSock *r)
{
	struct sk_buff *skb;

	if(rtf_get(r->toRT.fifo, &skb, sizeof(skb)) != sizeof(skb)) {
		return NULL;
	}
	return skb;
}

void
rtsock_enqueue_skb(RtSock *r, struct sk_buff *skb)
{
	rtf_put(r->fromRT.fifo, &skb, sizeof(skb));
	rtl_global_pend_irq(rtsock_irq);
}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,3,0)

static void rtsock_safe_destruct(struct sk_buff *skb)
{
	sock_put(skb->sk);
}

void 
rtsock_dereference_sock(struct RtSock *r, struct sock *sk)
{
	struct sk_buff *skb;

				// always dereference in kernel space
	skb = rtsock_alloc_skb(r);
	if(skb) {
		skb->sk = sk;
		skb->destructor = rtsock_safe_destruct;
		rtsock_free_skb(r, skb);
	} else {
				// unless all buffers are exhausted
		sock_put(sk);
	}
}

#endif

void
rtsock_free_skb(RtSock *r, struct sk_buff *skb)
{
	skb->skb_used += 2;
	rtf_put(r->fromRT.fifo, &skb, sizeof(skb));
	rtl_global_pend_irq(rtsock_irq);
}

struct file_operations rtsock_fops = {
	llseek:rtsock_lseek,
	read:rtsock_read,
	write:rtsock_write,
	ioctl:rtsock_ioctl,
	open:rtsock_open,
	release:rtsock_release
        		/* nothing more, fill with NULLs */
};

void cleanup_module(void);

int init_module(void)
{
	int  i, result, irq;

	printk("rtsock Copyright(c)1998-2002 Innomedia Inc. (R Kavaler)\n");
	result = register_chrdev(rtsock_major, "rtsock", &rtsock_fops);
	if(result < 0) {
		printk(KERN_WARNING "rtsock: can't get major %d\n", rtsock_major);
		return result;
	}
	if(rtsock_major == 0) {
		rtsock_major = result;		/* dynamic major codes */
	}
	if(rtsock_ndevs > RTSOCK_NDEVS) {
		rtsock_ndevs = RTSOCK_NDEVS;
	}
	for(i=0; i<rtsock_ndevs; i++) {
		rtsock_initialize(&rtsock_devices[i], i);
	}
	irq = rtl_get_soft_irq(rtsock_irq_handler, "rtsock");
	if(irq > 0) {
		rtsock_irq = irq;
	} else {
		cleanup_module();
		printk(KERN_WARNING "Can't get an irq for rtsock");
		return -EIO;
	}
	return 0;
}

void cleanup_module(void)
{
	int i;

	if(rtsock_irq) {
		free_irq(rtsock_irq, 0);
	}
	unregister_chrdev(rtsock_major, "rtsock");

	for(i=0; i<rtsock_ndevs; i++) {
		rtsock_deinitialize(&rtsock_devices[i]);
	}
}