rtsock-test.c

来自「fsmlabs的real time linux的内核」· C语言 代码 · 共 273 行

/*
 * rtsock-test.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.h>
#include <rtl_sched.h>
#include <rtl_sync.h>
#include <rtl_fifo.h>

#include <rtsock.h>

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


struct MySock {				// rt side structure "above" sock layer
					// one mysock per sock
	int index;
	int local1;
	int local2;
	int received;
	struct sock *sock;
	struct RtSock *rtsock;
};

struct packet {				// packet contents for sending
	long field1;
	long field2;
};


spinlock_t g_spinlock;			// a global spinlock
static pthread_t thread = NULL;		// global thread for this test
struct RtSock *g_rtsock;		// global rtsock
static int period = 500;		// period is 500us (i.e. 2KHz)

#define	MAX_MYSOCKS		16
struct MySock MySockList[MAX_MYSOCKS];	// a double-link list would be better!

void
rtprintf(char *format, ...)
{
        char s[256];
        va_list ap;

        va_start(ap, format);
        vsprintf(s, format, ap);
        va_end(ap);
        rtf_put(0, s, strlen(s));
}

int
rtsock_interface(void *vrtsock, struct sock *s, unsigned long arg)
{
	DECLARE_MUTEX(ioctlSemaphore);
	struct RtSock *rtsock = vrtsock, *srtsock;
	struct MySock *m;
	int fd, index, operation;
	int *intp = (int *) arg;

	get_user(fd, intp++);
	get_user(operation, intp++);

	if(operation == 0) {
				// insert new sock into rt list
				// look for unused mysocks
		down(&ioctlSemaphore);
		for(m=MySockList; m<MySockList+MAX_MYSOCKS; m++) {
			if(m->rtsock == NULL) {
				break;
			}
		}
		up(&ioctlSemaphore);
		index = m-MySockList;
		put_user(index, intp++);
	} else {
		get_user(index, intp++);
	}
	if(index < 0 || index >= MAX_MYSOCKS) {
		return -EINVAL;
	}
	m = &MySockList[index];
	switch(operation) {
	case 0:				// creates a new mysock
		m->index = index;
		m->sock = s;
		rtsock_reference_sock(rtsock, m->sock);
		m->sock->user_data = m;
		get_user(m->local1, intp++);
		get_user(m->local2, intp++);
		m->received = 0;
		m->rtsock = rtsock;	// makes it visible to rt side
		return 0;
	case 1:			// change parameters of a sock
			// rt mutex needed here mutex here
		write_lock(&g_spinlock);
		get_user(m->local1, intp++);
		get_user(m->local2, intp++);
		write_unlock(&g_spinlocal);
		return 0;
	case 2:			// delete sock from rt list
		m = &MySockList[index];
		srtsock = m->rtsock;
		m->rtsock = NULL;	// makes it invisible to rt side
		m->sock->user_data = NULL;
		rtsock_dereference_sock(srtsock, m->sock);
		return 0;
	}
	return -1;
}
                                  
static void *
mainloop(void *x)
{
	struct MySock *m;
	struct sk_buff *skb;
	struct packet *p;
	int  i, delta;
	hrtime_t reftime;

        pthread_make_periodic_np(pthread_self(), gethrtime(), period*1000);

	reftime = gethrtime();
        while(1) {
		pthread_wait_np();
		for(i=0; i<MAX_MYSOCKS; i++) {
			m = &MySockList[i];
			if(m->rtsock && m->local1) {
	// send a packet on every sock every tick

				skb = rtsock_alloc_skb(m->rtsock);
				if(!skb) {
					rtprintf("cannot alloc skb\n");
					continue;
				}
				skb_set_owner_w(skb, m->sock);
				p = (struct packet *) 
					__skb_put(skb, sizeof(struct packet));
				p->field1 = htonl(m->local1);
				p->field2 = htonl(m->local2);
				m->local2++;
				rtsock_enqueue_skb(m->rtsock, skb);
			}
		}

	// receive all packets to this rtsock
		while((skb=rtsock_dequeue_skb(g_rtsock))) {
			if((m=skb->sk->user_data)) {
				m->received++;
				if(m->received > 10000) {
					delta = (gethrtime()-reftime)/1000000;
					m->received = 0;
					rtprintf("%d.%03d received 10K packets on index %d\n", delta/1000, delta%1000, m->index);
				}
			}
			rtsock_free_skb(g_rtsock, skb);
		}
        }
        return NULL;
}

int init_module(void)
{
	pthread_attr_t attr;
	struct sched_param p;
	int  i;

	printk("rtsock-test Copyright(c)1998-2002 Innomedia Inc. (R Kavaler)\n");
	rtf_create(0, 8192);		// used for printf messages
        if((g_rtsock = rtsock_create(0, 128, 128, 256)) == NULL) {
		printk("rtpsock cannot create rtsock.\n");
		return -ENOMEM;
        }
	rtsock_register_callback(g_rtsock, rtsock_interface, g_rtsock);
	pthread_attr_init(&attr);
	p.sched_priority = 1;
	pthread_attr_setschedparam(&attr, &p);
	if(pthread_create(&thread, &attr, mainloop, NULL) < 0) {
		rtsock_destroy(g_rtsock);
		return -ENOMEM;
	}
	for(i=0; i<MAX_MYSOCKS; i++) {
		MySockList[i].rtsock = NULL;
	}
	return 0;
}

void cleanup_module(void)
{
	int  i;
	struct RtSock *srtsock;
	struct MySock *m;

	for(i=0; i<MAX_MYSOCKS; i++) {
		m = &MySockList[i];
		if(m->rtsock) {
			srtsock = m->rtsock;
			m->rtsock = NULL;	// makes it invisible to rt side
			m->sock->user_data = NULL;
			rtsock_dereference_sock(srtsock, m->sock);
		}
	}
	rtsock_destroy(g_rtsock);
	pthread_delete_np(thread);
	rtf_destroy(0);
}