/* * 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		NULLstruct 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 voidrtsock_data_ready0(struct sock *sk, int len){	rtsock_data_ready(&rtsock_devices[0], sk, len);}static voidrtsock_data_ready1(struct sock *sk, int len){	rtsock_data_ready(&rtsock_devices[1], sk, len);}static voidrtsock_data_ready2(struct sock *sk, int len){	rtsock_data_ready(&rtsock_devices[2], sk, len);}static voidrtsock_data_ready3(struct sock *sk, int len){	rtsock_data_ready(&rtsock_devices[3], sk, len);}static voidrtsock_data_ready4(struct sock *sk, int len){	rtsock_data_ready(&rtsock_devices[4], sk, len);}static voidrtsock_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 intrtsock_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 voidappend_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 voidrtsock_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 voidrtsock_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_trtsock_read(struct file *filp, char *buf, size_t count, loff_t *new){	return 0;}static ssize_trtsock_write(struct file *filp, const char *buf, size_t count, loff_t *new){	return 0;}static loff_trtsock_lseek(struct file *filp, loff_t off, int whence){	return 0;}static intrtsock_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;}voidrtsock_register_callback(RtSock *r, 	int (*callback)(void *, struct sock *, unsigned long),	void *callbackData){	r->callbackData = callbackData;	r->callback = callback;}voidrtsock_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;}voidrtsock_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);	}}#endifvoidrtsock_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]);	}}