rtsocku.c

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

/*
 * rtsocku.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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/poll.h>
#include <sys/time.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <netdb.h>
#include <asm/ioctl.h>

#include <rtsock.h>

#define	SENDRATE	2000		// 2000 packets per second

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

static int g_rtfd = -1;

int
DecodeIPAddress(char *s, struct sockaddr_in *sin)
{
	int  port;
	char ip[1024];
	struct hostent *he;

	memset(sin, 0, sizeof(*sin));
	sin->sin_family = AF_INET;
	switch(sscanf(s, "%1023[^:]:%i", ip, &port)) {
	case 0:
		if(sscanf(s, ":%i", &port) == 1) {
			sin->sin_port = htons(port);
		}
		break;
	case 1:
		port = 0;
	case 2:
		sin->sin_port = htons(port);
		he = gethostbyname(ip);
		if(he) {
			memcpy(&sin->sin_addr.s_addr, he->h_addr_list[0], 4);
		} else {
			return -1;
		}
		break;
	}
	return 0;
}

char *
EncodeIPAddress(char *s, struct sockaddr_in *sin)
{
	int x;

	x = ntohl(sin->sin_addr.s_addr);
	sprintf(s, "%d.%d.%d.%d:%d", 
		(x>>24)&0xFF, (x>>16)&0xFF, (x>>8)&0xFF, x&0xFF, 
		htons(sin->sin_port)
	);
	return s;
}

int
CreateUDPSocket()
{
	int sock;

	if((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		return -1;
	}
	return sock;
}

void
DestroySocket(int sock)
{
	if(sock < 0) {
		return;
	}
	close(sock);
}

int
BindSocket(int sock, char *s)
{
	struct sockaddr_in localAddr;

	if(sock < 0) {
		return sock;
	}
	if(DecodeIPAddress(s, &localAddr) < 0) {
		fprintf(stderr, "cannot resolve %s\n", s);
		return -1;
	}
	if(bind(sock, &localAddr, sizeof(localAddr)) < 0 ) {
		perror("bind failed");
		return -1;
	}
	return sock;
}

int
ConnectSocket(int sock, char *s) 
{
	struct sockaddr_in farAddr;

	if(sock < 0) {
		return sock;
	}
	if(DecodeIPAddress(s, &farAddr) < 0) {
		fprintf(stderr, "cannot resolve %s\n", s);
		return -1;
	}
	if(connect(sock, &farAddr, sizeof(farAddr)) < 0) {
		perror("connect failed");
		return -1;
	}
	return sock;
}

int
CreateRtSocket(int sock, int *params, int paramsLength)
{
	int *ioctlParams, index, i;

	if(sock < 0) {
		return sock;
	}
	if(g_rtfd < 0) {
		if((g_rtfd=open("/dev/rtsock0", O_RDWR)) < 0 ) {
		    perror("rtsock0 open");
		    return -1;
		}
	}
	ioctlParams = calloc(paramsLength+3, sizeof(int));
	ioctlParams[0] = sock;		// fd of sock
	ioctlParams[1] = 0;		// operation
	ioctlParams[2] = 0;		// index of mysock in rt space
	if(params) {
		for(i=0; i<paramsLength; i++) {
			ioctlParams[3+i] = params[i];
		}
	}
	if(ioctl(g_rtfd, RTSOCK_IOCINTERFACE, ioctlParams) < 0) {
		perror("ioctl(RTSOCK_IOCINTERFACE, insert)");
		free(ioctlParams);
		close(sock);
		return -1;
	}
	index = ioctlParams[2]; 	// this parameter is return by ioctl
	free(ioctlParams);
	return index;
}

void
DestroyRtSocket(int sock, int index)
{
	long ioctlParams[3];

	if(sock < 0 || index < 0) {
		return;
	}
	ioctlParams[0] = sock;
	ioctlParams[1] = 2;
	ioctlParams[2] = index;
	if(ioctl(g_rtfd, RTSOCK_IOCINTERFACE, ioctlParams) < 0) {
		perror("ioctl(RTSOCK_IOCINTERFACE, destroy)");
	}
}

int
RedirectRtSocket(int sock)
{
	long ioctlParams[1];

	if(sock < 0) {
		return sock;
	}
	ioctlParams[0] = sock;
	if(ioctl(g_rtfd, RTSOCK_IOCREDIRECTFD, ioctlParams) < 0) {
		perror("ioctl(RTSOCK_IOCINTERFACE, destroy)");
		return -1;
	}
	return sock;
}

int
ResetRtSocket(int sock)
{
	long ioctlParams[1];

	if(sock < 0) {
		return sock;
	}
	ioctlParams[0] = sock;
	if(ioctl(g_rtfd, RTSOCK_IOCRESETFD, ioctlParams) < 0) {
		perror("ioctl(RTSOCK_IOCINTERFACE, destroy)");
		return -1;
	}
	return sock;
}

int
UpdateRtSocket(int sock, int index, int *params, int paramsLength)
{
	int *ioctlParams, i;

	if(sock < 0 || index < 0) {
		return -1;
	}
	ioctlParams = calloc(paramsLength+3, sizeof(int));
	ioctlParams[0] = sock;		// fd of sock
	ioctlParams[1] = 1;		// operation
	ioctlParams[2] = index;		// index of mysock in rt space
	if(params) {
		for(i=0; i<paramsLength; i++) {
			ioctlParams[3+i] = params[i];
		}
	}
	if(ioctl(g_rtfd, RTSOCK_IOCINTERFACE, ioctlParams) < 0) {
		perror("ioctl(RTSOCK_IOCINTERFACE, update)");
		free(ioctlParams);
		return -1;
	}
	free(ioctlParams);
	return sock;
}

int
main(int argc, char *argv[])
{
	int  sock, rtsock, i, count, params[2], fd;
	int  addrLen;
	struct packet p;
	struct sockaddr_in addr;
	struct timeval tv1, tv2;
	char *s, s1[1024];

	if(argc < 2) {
		fprintf(stderr, "usage: rtsocku [ru|rr|tu|tr] [<address>]\n");
		exit(-1);
	}
	s = (argc < 3)? "" : argv[2];
	if(strcmp(argv[1], "ru") == 0) {
		if((sock = BindSocket(CreateUDPSocket(), s)) < 0) {
			exit(-1);
		}
		addrLen = sizeof(addr);
		if(getsockname(sock, &addr, &addrLen) < 0 ) {
		    	close(sock);
		    	return -1;
		}
		printf("rx bound to address %s\n", EncodeIPAddress(s1, &addr));

		count = 0;
		gettimeofday(&tv1, NULL);
		for(;;) {
			recv(sock, &p, sizeof(p), 0);
			count++;
			if(count >= 10000) {
				gettimeofday(&tv2, NULL);
				printf("%.3f received 10000 packets\n",
					(tv2.tv_sec-tv1.tv_sec) 
					+ tv2.tv_usec/1000000.0
				);
				count = 0;
			}
		}
	} else if(strcmp(argv[1], "rr") == 0) {
		if((fd = open("/dev/rtf0", O_RDONLY)) < 0) {
			perror("/dev/rtf0");
			exit(-1);
		}
		if((sock = BindSocket(CreateUDPSocket(), s)) < 0) {
			exit(-1);
		}
		addrLen = sizeof(addr);
		if(getsockname(sock, &addr, &addrLen) < 0 ) {
		    	close(sock);
		    	return -1;
		}
		printf("rx bound to address %s\n", EncodeIPAddress(s1, &addr));

		params[0] = 1;
		params[1] = 0;
		if((rtsock = CreateRtSocket(sock, params, 2)) < 0) {
			DestroySocket(sock);
			exit(-1);
		}
		if(RedirectRtSocket(sock) < 0) {
			DestroyRtSocket(sock, rtsock);
			DestroySocket(sock);
			exit(-1);
		}

		for(;;) {
			char buffer[1024];
			int  len;

			len = read(fd, buffer, sizeof(buffer)-1);
			if(len > 0) {
				buffer[len] = 0;
				printf("%s", buffer);
			}
		}

	} else if(strcmp(argv[1], "tu") == 0) {
		if((sock = ConnectSocket(BindSocket(CreateUDPSocket(), ""), s)) < 0) {
			exit(-1);
		}
		
		p.field1 = htonl(1);
		p.field2 = 0;
		gettimeofday(&tv1, NULL);
		for(;;) {
					// sleep for 1/SENDRATE
			tv1.tv_usec += 1000000/SENDRATE;
			if(tv1.tv_usec > 1000000) {
				tv1.tv_usec -= 1000000;
				tv1.tv_sec++;
			}
			gettimeofday(&tv2, NULL);
			i = (tv1.tv_usec - tv2.tv_usec) + 
				1000000 * (tv1.tv_sec - tv2.tv_sec);
			if(i > 0) {
				usleep(i);
			}

					// send 1 packet
			if(send(sock, &p, sizeof(p), 0) < 0) {
			    printf("send failed after %d packets\n", 
				ntohl(p.field2));
			    DestroySocket(sock);
			    exit(0);
			}
			p.field2 = htonl(ntohl(p.field2)+1);

		}
	} else if(strcmp(argv[1], "tr") == 0) {
		if((sock = ConnectSocket(BindSocket(CreateUDPSocket(), ""), s)) < 0) {
			exit(-1);
		}
		params[0] = 1;
		params[1] = 0;
		if((rtsock = CreateRtSocket(sock, params, 2)) < 0) {
			DestroySocket(sock);
			exit(-1);
		}
		for(;;) {
			struct pollfd pfd;

				// wait for an exception condition
			do {
				pfd.fd = sock;
				pfd.events = POLLERR;
				pfd.revents = 0;
				poll(&pfd, 1, 1000000);
			} while((pfd.revents & POLLERR) == 0);

				// got the exception
			printf("send failed\n");
			DestroyRtSocket(sock, rtsock);
			DestroySocket(sock);
			exit(0);
		}
	} else {
		fprintf(stderr, "usage: rtsocku [ru|rr|tu|tr] [<address>]\n");
		exit(-1);
	}
	return 0;
}